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⎦⎥⎥</