SLAM中的三角化方案

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

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

1. 数值求解方案

2. 几何求解方案

                              公式 1

                            公式 2

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

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在两个视图中的平面坐标，，以及两个视图的投影矩阵，。

求解过程： 

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）

其中：

                                  （式2）

而且：

2. 令：

                                                     （式3）

则：

3. 若设：

则：

                                                            （式4）

将式1代入式4得：

                                 

                                                                                                          （式5）

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

                                                                                                             （式6）

其中，不全为零

将式6代入式5得：

因为所以当时有最小值

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