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

淡淡的烟草味﹌ 2022-05-18 05:58 294阅读 0赞

ORB中三角化算法：

已知第一帧的位姿为![R\_W^\{C\_1\}][R_W_C_1]，![t\_W^\{C\_1\}][t_W_C_1],第二帧的位资为![R\_W^\{C\_2\}][R_W_C_2]，![t\_W^\{C\_2\}][t_W_C_2]，则地标点3D坐标的求解过程如下：

![70][]

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 )][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\_A][P_A]和![P\_B][P_B]分别属于点P在A帧和B帧中的归一化相机坐标系下的三维坐标（d=1），解的X后，分别为两帧的深度dA和dB, 则P在B帧中的坐标为：![P\_\{C\_B\} = P\_B\\cdot d\_B][P_C_B_ _ P_B_cdot d_B]，同理P在A帧中的坐标为：![P\_\{C\_A\} = P\_A\\cdot d\_A][P_C_A_ _ P_A_cdot d_A]，然后将B帧的P点投影到A帧为：

![P\_\{C\_A\}^\* = T\_\{AB\}\\cdot P\_\{C\_B\}][P_C_A_ _ T_AB_cdot P_C_B]

最后计算为均值![P\_A^\{'\} = \\frac\{ \\left ( P\_\{C\_A\} + P\_\{C\_A\}^\* \\right )\}\{2\}][P_A_ _ _frac_ _left _ P_C_A_ _ P_C_A_ _right _2]，作为该地标点在参考帧A中的估计值，而且在SVO中该点的深度值可以通过混合高斯分布进行迭代。

***总结：***

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

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

[R_W_C_1]: https://private.codecogs.com/gif.latex?R_W%5E%7BC_1%7D
[t_W_C_1]: https://private.codecogs.com/gif.latex?t_W%5E%7BC_1%7D
[R_W_C_2]: https://private.codecogs.com/gif.latex?R_W%5E%7BC_2%7D
[t_W_C_2]: https://private.codecogs.com/gif.latex?t_W%5E%7BC_2%7D
[70]: /images/20220518/9d6d78888ba54db1a9f9412d8c616b29.png
[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]: https://private.codecogs.com/gif.latex?%5Cleft%20%5B%20%5Cbegin%7Bmatrix%7D%20P_A%5ETP_A%20%26%20P_A%5ETR_%7BAB%7DP_B%5C%5C%20-P_B%5ETR_%7BBA%7DP_A%26-P_B%5ETP_B%20%5Cend%7Bmatrix%7D%20%5Cright%20%5D%5Ccdot%20X%20%3D%20%5Cleft%20%28%20%5Cleft%20%5B%20%5Cbegin%7Bmatrix%7D%20P_A%5ETt_%7BAB%7D%5C%5C%20P_B%5ETR_%7BBA%7Dt_%7BAB%7D%7D%5Cend%7Bmatrix%7D%20%5Cright%20%5D%20%5Cright%20%29
[P_A]: https://private.codecogs.com/gif.latex?P_A
[P_B]: https://private.codecogs.com/gif.latex?P_B
[P_C_B_ _ P_B_cdot d_B]: https://private.codecogs.com/gif.latex?P_%7BC_B%7D%20%3D%20P_B%5Ccdot%20d_B
[P_C_A_ _ P_A_cdot d_A]: https://private.codecogs.com/gif.latex?P_%7BC_A%7D%20%3D%20P_A%5Ccdot%20d_A
[P_C_A_ _ T_AB_cdot P_C_B]: https://private.codecogs.com/gif.latex?P_%7BC_A%7D%5E*%20%3D%20T_%7BAB%7D%5Ccdot%20P_%7BC_B%7D
[P_A_ _ _frac_ _left _ P_C_A_ _ P_C_A_ _right _2]: https://private.codecogs.com/gif.latex?P_A%5E%7B%27%7D%20%3D%20%5Cfrac%7B%20%5Cleft%20%28%20P_%7BC_A%7D%20&plus;%20P_%7BC_A%7D%5E*%20%5Cright%20%29%7D%7B2%7D