VSLAM预备知识 ICP PnP
一、3D-3D:ICP
已知n个3D点，在两个坐标轴下的坐标分别为和
，已匹配好，求外参sR，t。其中s为两个坐标系的尺度比，R为旋转矩阵，t为平移向量。
SVD方法：
存在关系
构建误差的最小二乘，
推导可得:
其中和
为质心，因此交叉项乘积为0.故
一部分只和R有关，一部分和R，t有关。先求R，再求t。因此求解过程总结如下：
在第二步中展开，得到
非线性优化：以迭代求最小误差的方式。
二、3D-2D:PnP
当我们知道n 个 3D 空间点以及它们的投影位置(2D)时，如何估计相机所在的位姿。
最少需要3对点（以及至少一个额外点验证结果）
算法大致有如下几种：
直接线性变换DLT：
最少需要六对匹配点，求出的旋转矩阵不一定满足约束，需要取近似。
P3P
仅需要3对匹配点（额外一对验证点）
这里得到了在相机坐标系下的3D坐标，因此也就变成了3D-3D问题，使用ICP即可求解R，t。
三、三角测量Triangulation 2D-3D
已知多个视角匹配好的2D点（u,v)，以及这多个视角的位姿，求解点的3D坐标。
由于相机位姿已知，因此世界坐标系下的坐标也易得。
特殊情况：双目，即两个相机的位姿只差一个平移。
四、2D-2D 对极几何
通过匹配好的很多对2D点，得到R，t
基础矩阵 Fundamental模型
E=t^R共6个自由度（9个变量的矩阵），考虑尺度等价性，因此5个自由度最少需要用5对点。也可以只考虑尺度等级性，使用八点法。