详解SLAM中的两种常用的三角化求解地标点的方法

ORB中三角化算法：

已知第一帧的位姿为，,第二帧的位资为，，则地标点3D坐标的求解过程如下：

OKVIS中的三角化算法：

                                                         

其中和分别属于点P在A帧和B帧中的归一化相机坐标系下的三维坐标（d=1），解的X后，分别为两帧的深度dA和dB, 则P在B帧中的坐标为：，同理P在A帧中的坐标为：，然后将B帧的P点投影到A帧为：

                                                                                           

最后计算为均值，作为该地标点在参考帧A中的估计值，而且在SVO中该点的深度值可以通过混合高斯分布进行迭代。

 

总结：

可以看出ORB中求解方法较为简单，且利用SVD方法，近似的求解了地标点的在参考帧中的坐标，用了最小二乘的思想。

而OKVIS中将地标点在两帧中的深度同时求解，并取平均作为最终的地标点估计值。