三角形法恢复空间点深度

计算机视觉

通常，在已知两个相机的相对位姿$\mathbf T$的情况下，得到在两个视图下的对应匹配点$\bf x \leftrightarrow \bf x'$，我们就可以求得该对应点在空间中的位置，也就是求得图像点的深度。接下来介绍两种求解方法。

1.求解空间点坐标

当我们得到两个视图的一组匹配点，我们希望能恢复出世界点在三维世界的坐标。这里就涉及到使用三角形法来恢复点在3D空间的结构。一般比较常用的方法是线性三角形法（Linear triangulation methods ）。线性三角形法使用直接线性变化（DLT）对点的世界坐标进行求解。

已知点对$\bf x$和$\bf x'$和两个图像的投影矩阵$P$和$P'$ ，根据相机投影模型，对应3D点$\bf X$满足

$$\begin{equation} \mathbf x = P \mathbf X \qquad \mathbf x'= P'\mathbf X \end{equation}$$

使用DLT我们需要把式子改变成$A\mathbf X=\mathbf 0$的形式。由于是齐次坐标的表示形式，使用叉乘消去齐次因子，有

$$\begin{equation} \mathbf x \times (P \mathbf X) = \mathbf 0 \qquad \mathbf x' \times (P'\mathbf X)=\mathbf 0 \end{equation}$$

把$P$和$P'$按照行展开代入，对第一幅图有

$$\begin{equation} \begin{bmatrix} 0 & -1 & y\\ 1 & 0 & -x\\ -y & x & 0 \end{bmatrix} \begin{bmatrix} P^{1T}X\\P^{2T}X\\P^{3T}X \end{bmatrix} = 0 \end{equation}$$

即

$$\begin{equation} \begin{split} x(P^{3T}X)-(P^{1T})=0\\ y(P^{3T}X)-(P^{2T})=0\\ x(P^{2T}X)-y(P^{1T})=0\\ \end{split} \end{equation}$$

可见第三个式子可以由上两个式子线性表示，所以只需要取前连个式子即可，从而有形如$A\mathbf X=\mathbf 0$的方程，其中

$$\begin{equation} A =\begin{bmatrix} x(P^{3T}X)-(P^{1T})\\ y(P^{3T}X)-(P^{2T})\\ x'(P'^{3T}X)-(P'^{1T})\\ y'(P'^{3T}X)-(P'^{2T})\\ \end{bmatrix} \end{equation}$$

由于$\bf X$是自由度为3的齐次方程，所以这是一个冗余的方程，这里相当于解一个线性最小二乘问题。方程的解为$A$的最小奇异值对应的单位奇异矢量，解得$X=(x,y,z,w)$，则最后令$X$缩放使得的最后一项为1即可得到我们所求的3D点$\bf X$的坐标。

ORB-SLAM2[1]中的三角形法的代码如下：

void Initializer::Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D)
{
    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<float>(3);
}

2.求解空间点深度

在深度滤波中经常使用这种形式，通常把首先观测到某一个图像点$\mathbf{x}$的图像帧设置为参考帧，在其他的图像帧中上做对极线，在对极线上搜索匹配点$\mathbf{x'}$，然后通过匹配点进行三角法恢复深度。我们设相机内参为$K$，则有：

$$\begin{equation} \mathbf f = K^{-1}\mathbf x \qquad \mathbf f' = K^{-1}\mathbf x' \end{equation}$$
这里的$\mathbf{f}$和$\mathbf{f'}$是单位平面上的点。通过点的对应关系，我们有：
$$\begin{equation} z' \cdot \mathbf f' = \left[\mathbf R| \mathbf t\right] z \cdot \mathbf f = z \cdot \mathbf R\mathbf f + \mathbf t \end{equation}$$
这里的$z$和$z'$分别对应空间点在两个相机坐标系下的深度。于是我们可以把公式化为：
$$\begin{equation} \begin{pmatrix}-\mathbf R\mathbf f & \mathbf f'\end{pmatrix}\begin{pmatrix}z \\ z'\end{pmatrix} = \mathbf t \end{equation}$$
这里就是$\mathbf{A}\mathbf{x}=\mathbf{b}$的形式，要解该方程，可以用正规方程来求解，也就是解$\mathbf{A}^T(\mathbf{A}\mathbf{x}-\mathbf{b})=\mathbf0$，也就是有：
$$\begin{equation} \mathbf{A}^T\mathbf{A}\mathbf{x}=\mathbf{A}^T\mathbf{b} \end{equation}$$
所以解得:
$$\begin{equation} \mathbf{x} = (\mathbf{A}^T\mathbf{A})^{-1}\mathbf{A}^T\mathbf{b} \end{equation}$$

在SVO[2]中的实现如下，返回在参考帧下点的深度：

bool depthFromTriangulation(
    const SE3& T_search_ref,
    const Vector3d& f_ref,
    const Vector3d& f_cur,
    double& depth)
{
  Matrix<double,3,2> A; A << T_search_ref.rotation_matrix() * f_ref, f_cur;
  const Matrix2d AtA = A.transpose()*A;
  if(AtA.determinant() < 0.000001)
    return false;
  // d = - (ATA)^(-1) * AT * t
  const Vector2d depth2 = - AtA.inverse()*A.transpose()*T_search_ref.translation();
  depth = fabs(depth2[0]);
  return true;
}

由于解向量$\mathbf x$是二维的，其实在得到公式$(9)$之后，我们也可以直接采用克莱默法则求解。在REMODE[3]和《SLAM十四讲》中单目稠密重建的代码[4]的实现就是这样，其中REMODE代码如下，返回在参考帧下的空间点位置：

float3 triangulatenNonLin(
    const float3 &bearing_vector_ref,
    const float3 &bearing_vector_curr,
    const SE3<float> &T_ref_curr)
{
  const float3 t = T_ref_curr.getTranslation();
  float3 f2 = T_ref_curr.rotate(bearing_vector_curr);
  const float2 b = make_float2(dot(t, bearing_vector_ref),
                               dot(t, f2));
  float A[2*2];
  A[0] = dot(bearing_vector_ref, bearing_vector_ref);
  A[2] = dot(bearing_vector_ref, f2);
  A[1] = -A[2];
  A[3] = dot(-f2, f2);
  const float2 lambdavec = make_float2(A[3]*b.x - A[1]*b.y,
      -A[2]*b.x + A[0]*b.y) / (A[0]*A[3] - A[1]*A[2]);
  const float3 xm = lambdavec.x * bearing_vector_ref;
  const float3 xn = t + lambdavec.y * f2;
  return (xm + xn)/2.0f;
}

---

参考

• 泡泡机器人ORB-SLAM2注释
• Multiple View Geometry in Computer Vision,Second Edition, 12.2 P312 & A5.2.1 P591