SLAM中三角化推导

文章目录

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

SLAM三角化 问题描述

对于单目相机而言，一般通过两帧图像匹配计算出所对应的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 ] \lambda\begin{bmatrix} u\v\1 \end{bmatrix} =\overbrace{K\begin{bmatrix} R\vert 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 ] \lambda\begin{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
上面等式只能提供两个约束，两个匹配像素点共提供四个约束：
[ u 1 T 3 1 − T 1 1 v 1 T 3 1 − T 2 1 u 2 T 3 2 − T 1 2 v 2 T 3 2 − T 2 2 ] P = 0 \begin{bmatrix} u_1T_3^1-T_1^1\ v_1T_3^1-T_2^1\ u_2T_3^2-T_1^2\ v_2T_3^2-T_2^2\ \end{bmatrix}P = 0 ⎣⎢⎢⎡​u1​T31​−T11​v1​T31​−T21​u2​T32​−T12​v2​T32​−T22​​⎦⎥⎥⎤​P=0

求解方法

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

设:
S = [ u 1 T 3 1 − T 1 1 v 1 T 3 1 − T 2 1 u 2 T 3 2 − T 1 2 v 2 T 3 2 − T 2 2 ] S = \begin{bmatrix} u_1T_3^1-T_1^1\ v_1T_3^1-T_2^1\ u_2T_3^2-T_1^2\ v_2T_3^2-T_2^2\ \end{bmatrix} S=⎣⎢⎢⎡​u1​T31​−T11​v1​T31​−T21​u2​T32​−T12​v2​T32​−T22​​⎦⎥⎥⎤​
对矩阵 S S S进行奇异值分解得： S = U Σ V T S=U\Sigma V^T S=UΣVT，于是：
P = s V 4 T P = sV^T_{4} P=sV4T​
即右奇异矩阵最后一行的转置，需要注意的是最后还要一个缩放因子 s s s，将 P P P的最后一维数值转为1。

c++代码

实现比较简单，主要使用的是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);