SLAM中的三角化方案

Linear triangulation methods 又称为线性三角形法,其作用是通过同一个三维空间点在两个视图中的平面坐标(也可以认为是像素坐标),求解这个三维空间点的世界坐标。通常的求解方式有两种,一种是数值求解;一种是几何求解。

首先给出问题:有一个三维空间点X,X分别被两帧图像观测到,这两帧图像拍摄时刻的相机姿态分别为P_cam1_w和P_cam2_w,将X在cam1坐标系下的单位向量表示f_{1},将X在cam2坐标系下的单位向量表示f_{2};P_cam1_w和P_cam2_w为3行4列的矩阵表示,由3行3列的旋转矩阵和3行1列的平移矩阵组成。

1. 数值求解方案

2. 几何求解方案

SLAM中的三角化方案_第1张图片

L_{1} = L_{2} cos(\gamma ) + \begin{vmatrix} t_{12} \end{vmatrix}cos(\alpha )

L_{2} = L_{1} cos(\gamma ) + \begin{vmatrix} t_{12} \end{vmatrix}cos(\beta )

cos(\gamma ) = f_{1}.dot(f_{2})

cos(\alpha ) = t_{12}.dot(f_{1}) / \begin{vmatrix}t_{12} \end{vmatrix}

cos(\beta ) = -t_{12}.dot(f_{2}) / \begin{vmatrix}t_{12} \end{vmatrix}

L_{1} - L_{2} cos(\gamma ) = \begin{vmatrix} t_{12} \end{vmatrix}cos(\alpha )

L_{1} - L_{2}f_{1}.dot(f_{2}) = t_{12}.dot(f_{1})                              公式 1

L_{1} cos(\gamma ) - L_{2} = - \begin{vmatrix} t_{12} \end{vmatrix}cos(\beta )

L_{1} f_{1}.dot(f_{2}) - L_{2} =t_{12}.dot(f_{2})                             公式 2

最终由公式1和公式2得出:

\begin{bmatrix} f_{1}.dot(f_{1}), & -f_{1}.dot(f_{2}) \\ f_{1}.dot(f_{2}),& -f_{2}.dot(f_{2}) \end{bmatrix} \begin{bmatrix} L_{1}\\ L_{2} \end{bmatrix} = \begin{bmatrix} t_{12}.dot(f_{1})\\ t_{12}.dot(f_{2}) \end{bmatrix}

3. 参考代码实现

#include 
#include 

opengv::point_t
opengv::triangulation::triangulate(
    const relative_pose::RelativeAdapterBase & adapter,
    size_t index )
{
  translation_t t12 = adapter.gett12();
  rotation_t R12 = adapter.getR12();
  Eigen::Matrix P1 = Eigen::Matrix::Zero();
  P1.block<3,3>(0,0) = Eigen::Matrix3d::Identity();
  Eigen::Matrix P2 = Eigen::Matrix::Zero();
  P2.block<3,3>(0,0) = R12.transpose();
  P2.block<3,1>(0,3) = -R12.transpose()*t12;
  bearingVector_t f1 = adapter.getBearingVector1(index);
  bearingVector_t f2 = adapter.getBearingVector2(index);

  Eigen::MatrixXd A(4,4);
  A.row(0) = f1[0] * P1.row(2) - f1[2] * P1.row(0);
  A.row(1) = f1[1] * P1.row(2) - f1[2] * P1.row(1);
  A.row(2) = f2[0] * P2.row(2) - f2[2] * P2.row(0);
  A.row(3) = f2[1] * P2.row(2) - f2[2] * P2.row(1);

  Eigen::JacobiSVD< Eigen::MatrixXd > mySVD(A, Eigen::ComputeFullV );
  point_t worldPoint;
  worldPoint[0] = mySVD.matrixV()(0,3);
  worldPoint[1] = mySVD.matrixV()(1,3);
  worldPoint[2] = mySVD.matrixV()(2,3);
  worldPoint = worldPoint / mySVD.matrixV()(3,3);

  return worldPoint;
};

opengv::point_t
opengv::triangulation::triangulate2(
    const relative_pose::RelativeAdapterBase & adapter,
    size_t index )
{
  translation_t t12 = adapter.gett12();
  rotation_t R12 = adapter.getR12();
  bearingVector_t f1 = adapter.getBearingVector1(index);
  bearingVector_t f2 = adapter.getBearingVector2(index);

  bearingVector_t f2_unrotated = R12 * f2;
  Eigen::Vector2d b;
  b[0] = t12.dot(f1);
  b[1] = t12.dot(f2_unrotated);
  Eigen::Matrix2d A;
  A(0,0) = f1.dot(f1);
  A(1,0) = f1.dot(f2_unrotated);
  A(0,1) = -A(1,0);
  A(1,1) = -f2_unrotated.dot(f2_unrotated);
  Eigen::Vector2d lambda = A.inverse() * b;
  Eigen::Vector3d xm = lambda[0] * f1;
  Eigen::Vector3d xn = t12 + lambda[1] * f2_unrotated;
  point_t point = ( xm + xn )/2;
  return point;
};

https://github.com/ethz-asl/opengv/blob/master/src/triangulation/methods.cpp

4.延申思考

相对于数值求解方案,X在两个相机中的表示除了单位向量,也可以是像素坐标,此时P矩阵就是内参矩阵和转换矩阵的联合表达。

已知条件为:空间点的世界坐标X,X在两个视图中的平面坐标,以及两个视图的投影矩阵P_{1}

求解过程: 

1. 根据相机模型可以得出:

                    ,      

2. 对上面公式同时进行叉乘计算:

                   ,    

3. 根据向量本身的叉乘为零向量,得:

                               ,         

4. 将向量叉乘表示为矩阵形式为:

                

5. 将按行展开,代表矩阵第一行:

                

6. 展开上面的公式:

                

                

                

7. 可以发现上面的式3和式1式2是线性相关的,只取式1式2可以表示成

                

8. 此时相当于求解最小二乘问题,后面将讲解一下如何使用奇异值分解求解AX=0的最小二乘问题。

---------------------------------------------------------------------------------------------------------

---------------------------------------------------------------------------------------------------------

---------------------------------------------------------------------------------------------------------

对于AX=b的最小二乘求解:

                

但是当b=0时,像上面的求解空间点的世界坐标,X总是零向量,此时可以使用SVD分解进行求解,下面论证下,SVD求解最小二乘的可行性:证明的最小二乘解是A经过SVD分解后的V的第一列向量。

1. A经过SVD分解后,可以得:

                                             (式1)

其中:

SLAM中的三角化方案_第2张图片                                  (式2)

而且:

2. 令:

SLAM中的三角化方案_第3张图片                                                     (式3)

则:

3. 若设:

则:

                                                            (式4)

将式1代入式4得:

                                 

                                                                                                          (式5)

因为是正交矩阵,所以V的列向量是的一组标准正交基,故可以由表示:

                                                                                                             (式6)

其中不全为零

将式6代入式5得:

因为所以当有最小值

此时X等于V的第一列向量

你可能感兴趣的:(SLAM,Math,自动驾驶,矩阵,线性代数,视觉检测,人工智能)