三角测量原理及代码实现

背景知识

在使用对极几何求解本质矩阵或者基础矩阵后就可以恢复出旋转矩阵和平移向量了，但是由于本质矩阵E的尺度等价性，分解得到的平移向量t具有一个尺度，换言之，在分解过程中，t乘以任意非零常数，分解都是成立的，因此，通常把t进行归一化，让平移向量t的长度为1。
尺度不确定性，对t长度的归一化，直接导致了单目视觉的尺度不确定性。例如，程序中输出的t第一维约为0.822。这个0.822究竟是指0.822米还是0.822厘米，我们是没法确定的。因为对t乘以任意比例常数后，对极约束依然是成立的。换言之，在单目SLAM中，对轨迹和地图同时缩放任意倍数，我们得到的图像依然是一样的。这在第2讲中就已经向读者介绍过了。
在单目视觉中，我们对两张图像的t归一化相当于固定了尺度。虽然我们不知道它的实际长度是多少，但我们以这时的t为单位1，计算相机运动和特征点的3D位置。这被称为单目SLAM的初始化。在初始化之后，就可以用3D-2D计算相机运动了。初始化之后的轨迹和地图的单位,就是初始化时固定的尺度。因此,单目SLAM有一步不可避免的初始化。初始化的两张图像必须有一定程度的平移，而后的轨迹和地图都将以此步的平移为单位。
除了对t进行归一化，另一种方法是令初始化时所有的特征点平均深度为1，也可以固定一个尺度。相比于令t长度为1的做法，把特征点深度归一化可以控制场景的规模大小，使计算在数值上更稳定。

从E分解到R,t的过程中，如果相机发生的是纯旋转，导致t为零，那么，得到的E也将为零,这将导致我们无从求解R。不过，此时我们可以依靠H求取旋转，但仅有旋转时，我们无法用三角测量估计特征点的空间位置（这将在下文提到)，于是，另一个结论是，单目初始化不能只有纯旋转，必须要有一定程度的平移。如果没有平移，单目将无法初始化。在实践中，如果初始化时平移太小，会使得位姿求解与三角化结果不稳定，从而导致失败。相对地，如果把相机左右移动而不是原地旋转，就容易让单目SLAM初始化。因而，有经验的SLAM研究人员，在单目SLAM情况下经常选择让相机进行左右平移以顺利地进行初始化。

三角测量理论基础

之前两节我们使用对极几何约束估计了相机运动，也讨论了这种方法的局限性。在得到运动之后，下一步我们需要用相机的运动估计特征点的空间位置。在单目SLAM中，仅通过单张图像无法获得像素的深度信息，我们需要通过三角测量（Triangulation )(或三角化)的方法估计地图点的深度，如图7-11所示。

三角测量是指，通过不同位置对同一个路标点进行观察，从观察到的位置推断路标点的距离。三角测量最早由高斯提出并应用于测量学中,它在天文学、地理学的测量中都有应用。例如,我们可以通过不同季节观察到的星星的角度，估计它与我们的距离。在SLAM中,我们主要用三角化来估计像素点的距离。

我有疑问，就是右式仅仅包含s₁，为什么说右侧可以看成是s₂的方程。

上代码：（完整代码在视觉SLAM十四讲中给出，这里只给出关键代码及注释）

/**
 * 正常在slam14讲里变换矩阵是4×4的，但是在opencv里，是3*4的，直接就是旋转n矩阵和i平移向量组成
 *Mat T1 第一台相机的3x4投影矩阵，即该矩阵将世界坐标系中给定的3D点投影到第一张图像中
 *Mat T1 第二台相机的3x4投影矩阵，即该矩阵将世界坐标系中给定的3D点投影到第二幅图像中。
*/
void triangulation(
  const vector<KeyPoint> &keypoint_1,
  const vector<KeyPoint> &keypoint_2,
  const std::vector<DMatch> &matches,
  const Mat &R, const Mat &t,
  vector<Point3d> &points) {
  Mat T1 = (Mat_<float>(3, 4) <<
    1, 0, 0, 0,
    0, 1, 0, 0,
    0, 0, 1, 0);
  Mat T2 = (Mat_<float>(3, 4) <<
    R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), t.at<double>(0, 0),
    R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), t.at<double>(1, 0),
    R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2), t.at<double>(2, 0)
  );

  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);

  vector<Point2f> pts_1, pts_2;

  // pts_1 第一个图像中的特征点的2xN阵列。在c++版本的情况下，它也可以是特征点的向量或大小为1xN或Nx1的双通道矩阵。
  // pts_2 第二个图像中对应点的2xN阵列。在c++版本的情况下，它也可以是特征点的向量或大小为1xN或Nx1的双通道矩阵。
  // pts_1和pts_2不是像素坐标，而是相机坐标

  vector<Point2f> pts_1, pts_2;
  for (DMatch m:matches) {
    // 将像素坐标转换至相机坐标
    pts_1.push_back(pixel2cam(keypoint_1[m.queryIdx].pt, K));
    pts_2.push_back(pixel2cam(keypoint_2[m.trainIdx].pt, K));
  }

  /**
   * 变量 pts_4d 是一个四维数组，其中存储了通过三角测量得到的三维点的坐标。
   * 在三维计算机视觉中，这些坐标通常表示为齐次坐标（homogeneous coordinates），
   * 其包含了三维点的 X、Y、Z 坐标以及一个额外的缩放因子 W。
   * 具体来说，pts_4d 是一个 4xN 的矩阵，每一列代表一个三维点的齐次坐标 [X, Y, Z, W]。
   * 通过将齐次坐标除以 W，可以得到三维点的真实坐标 [X/W, Y/W, Z/W]。
  */


  Mat pts_4d;
  cv::triangulatePoints(T1, T2, pts_1, pts_2, pts_4d);

  // 转换成非齐次坐标
  for (int i = 0; i < pts_4d.cols; i++) {
    Mat x = pts_4d.col(i);
    x /= x.at<float>(3, 0); // 归一化
    Point3d p(
      x.at<float>(0, 0),
      x.at<float>(1, 0),
      x.at<float>(2, 0)
    );
    points.push_back(p);
  }
}

总结

三角测量是由平移得到的，有平移才会有对极几何中的三角形，才谈得上三角测量，因此，纯旋转是无法使用三角测量的，在平移存在的条件下，当平移很小时，像素匹配的不确定性讲导致较大的深度估计的不确定性，因此，想要提高三角化精度，一方面是提高特征点的提取和匹配精度，一方面是使平移增大，但是平移加大，使图像的外观发生明显的变化，从而使图像匹配变得困难，也就是说，平移太大容易导致匹配失败，平移太小，三角化精度不够，这就是三角化矛盾。
在单目SLAM中，由于图像没有深度信息，我们要等待特征点被追踪几帧后，产生了组后的视角，再用三角化来确定新增特征点的深度值，称为延迟三角化，但是如果相机发生了原地旋转，导致视差很小，那么就不好估计新观测到的特征点的深度，这种情况在机器人场合下非常常见，因为原地旋转是机器人常见的指令，在这种情况下，单目视觉可能出现追踪失败和尺度不正确的情况。