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

ORB中三角化算法:

已知第一帧的位姿为R_W^{C_1}t_W^{C_1},第二帧的位资为R_W^{C_2}t_W^{C_2},则地标点3D坐标的求解过程如下:

详解SLAM中的两种常用的三角化求解地标点的方法_第1张图片

OKVIS中的三角化算法:

                                                         \left [ \begin{matrix} P_A^TP_A & P_A^TR_{AB}P_B\\ -P_B^TR_{BA}P_A&-P_B^TP_B \end{matrix} \right ]\cdot X = \left ( \left [ \begin{matrix} P_A^Tt_{AB}\\ P_B^TR_{BA}t_{AB}}\end{matrix} \right ] \right )

其中P_AP_B分别属于点P在A帧和B帧中的归一化相机坐标系下的三维坐标(d=1),解的X后,分别为两帧的深度dA和dB, 则P在B帧中的坐标为:P_{C_B} = P_B\cdot d_B,同理P在A帧中的坐标为:P_{C_A} = P_A\cdot d_A,然后将B帧的P点投影到A帧为:

                                                                                           P_{C_A}^* = T_{AB}\cdot P_{C_B}

最后计算为均值P_A^{'} = \frac{ \left ( P_{C_A} + P_{C_A}^* \right )}{2},作为该地标点在参考帧A中的估计值,而且在SVO中该点的深度值可以通过混合高斯分布进行迭代。

 

总结:

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

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

你可能感兴趣的:(slam)