工具坐标6点法_VSLAM中的X点法汇总

在VSLAM当中，我们经常会接触一些算法叫做X点法，如常见的五点法、八点法等，它们其实是解决不同问题的不同算法，只是名字容易搞混，所以，在这里将常见的X点法梳理一遍。

二点法：P2P

P2P是一种4DoF PnP算法，常用在特征点匹配中剔除outlier（Two-Point RANSAC），也可以用来估计平移量。我们知道PnP是根据3D-2D匹配计算旋转和平移的一类算法，它是求解如下等式：

其中

为3D点，

为

在图像上的投影，

为内参，PnP已知多对3D-2D匹配对

来求解旋转

和平移

。通常，PnP至少需要3组匹配对才能求解出

，但如果旋转

已知（例如通过陀螺仪积分获得），则只需要两个点就可以求解出平移向量，所以叫二点法。

由于

已知，我们令

，将

乘到等式左边，令

（

相当于相机normalize平面的投影），则等式可以化简为

，即：

将第三行

替代到前两行可得：

共有

三个未知数，每组3D-2D点只能得到两个约束方程，所以至少需要两组3D-2D点（4个约束方程）才能求解出

。

进一步说，如果3D-2D点在平行的平面上，还可以计算出绕平面法向量的旋转。例如在无人机对地VO中，假设特征点都在地面上，无人机姿态近似水平，则给定两组3D-2D匹配对

，可以直接计算向量

与

之间的夹角来求解yaw角，然后再用上面的方法求解平移量，相当于做4DoF（yaw和t）VO。

三点法

a. 3D-3D位姿匹配，ICP

已知N对3D-3D匹配对

，求解最优的旋转

和平移

使得匹配对之间的最小二乘误差最小：

该最小二乘问题可以利用SVD求出解析解，最少需要3对“不共线”的3D点才能求解（具体推导参见《视觉SLAM14讲》中7.9节）。求解步骤如下：

1. 匹配点中心化（减去点的均值）

2. 求最优旋转矩阵

：