SLAM中三角化推导

• SLAM三角化
• • 问题描述
  • 问题推导
  • 求解方法
  • c++代码

对于单目相机而言，一般通过两帧图像匹配计算出所对应的3D点进行建图。

问题的输入：变换矩阵 R R R，位移 t t t，相机参数 K K K，对应图像中的像素点 p = [ u , v , 1 ] T mathscr{p}=[u,v,1]^T p=[u,v,1]T。这个变换指的是相机坐标变换到世界坐标系的变换。

问题的输出：世界坐标系下一点坐标 P = [ X , Y , Z , 1 ] T P=[X,Y,Z,1]^T P=[X,Y,Z,1]T。

由投影模型:
λ [ u v 1 ] = K [ R ∣ t ] ⏞ T [ X Y Z 1 ] lambdabegin{bmatrix} u\v\1 end{bmatrix} =overbrace{Kbegin{bmatrix} Rvert t end{bmatrix} }^T begin{bmatrix} X\Y\Z\1 end{bmatrix} λ⎣⎡​uv1​⎦⎤​=K[R∣t​] ​T​⎣⎢⎢⎡​XYZ1​⎦⎥⎥⎤​
于是:
λ [ u v 1 ] = T [ X Y Z 1 ] lambdabegin{bmatrix} u\v\1 end{bmatrix} =T begin{bmatrix} X\Y\Z\1 end{bmatrix} λ⎣⎡​uv1​⎦⎤​=T⎣⎢⎢⎡​XYZ1​⎦⎥⎥⎤​
两边叉乘 p mathscr{p} p，得：
[ 0 − 1 v 1 0 − u − v u 0 ] [ T 1 T 2 T 3 ] P = 0 begin{bmatrix} 0 & -1 & v\ 1 & 0& -u\ -v & u & 0 end{bmatrix} begin{bmatrix} T_1\T_2\T_3 end{bmatrix}P= 0 ⎣⎡​01−v​−10u​v−u0​⎦⎤​⎣⎡​T1​T2​T3​​⎦⎤​P=0
最后得到线性方程：
[ − T 2 + v T 3 T 1 − u T 3 − v T 1 + u T 2 ] P = 0 begin{bmatrix} -T_2+vT_3\ T_1-uT_3\ -vT_1+uT_2 end{bmatrix}P=0 ⎣⎡​−T2​+vT3​T1​−uT3​−vT1​+uT2​​⎦⎤​P=0
一个像素点提供三个约束，可以直接解除3D点 P P P。

这里我们可以使用SVD分解，对这个问题进行求解：

设:
S = [ − T 2 + v T 3 T 1 − u T 3 − v T 1 + u T 2 ] S=begin{bmatrix} -T_2+vT_3\ T_1-uT_3\ -vT_1+uT_2 end{bmatrix} S=⎣⎡​−T2​+vT3​T1​−uT3​−vT1​+uT2​​⎦⎤​
对矩阵 S S S进行奇异值分解得： S = U Σ V T S=USigma V^T S=UΣVT，于是：
P = s V 3 T P = sV^T_{3} P=sV3T​
即右奇异矩阵最后一行的转置，需要注意的是最后还要一个缩放因子 s s s，将 P P P的最后一维数值转为1。

实现比较简单，主要使用的是SVD分解，这里直接截取ORB_SLAM2中三角化代码:

cv::Mat A(4,4,CV_32F);
A.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);
A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);
A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);
A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);
cv::Mat u,w,vt;
cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
x3D = vt.row(3).t();
x3D = x3D.rowRange(0,3)/x3D.at(3);