在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节)。