三角化求空间点

前言

---

在视觉追踪中或者SLAM中，不可避免的使用到三角化求空间点的坐标的过程，已知条件不同，求解空间点坐标的方法也就不同，因此在这里就总结一下，欢迎讨论。

方法一

---

已知：两个视图一组匹配点$x_1$和$x_2$，每个视图的相机投影矩阵映射关系$P_1$和$P_2$.
求：3D点的空间坐标。
解法：根据如下公式很容易求得X，$$x_1=P_{1}X$$
同理，$$x_2=P_{2}X$$也非常容易求得$X$得3D点坐标.如果投影矩阵$P_1、P_2$不是针对同一个世界坐标系，求得3D点坐标本身就没啥意义。如果针对的是同一个世界坐标系，由于误差的影响，$X$的坐标并不可能完全一致。
而另外一种算法：
$$\begin{gathered} x_1\times (P_{1}X)=0 \\ {x}_2\times (P_{2}X)=0 \end{gathered}$$
展开
$$\left[\begin{array}{ccc}0& -1& y\\ 1& 0& -x\\ -y& x& 0\end{array}\right]\left[\begin{array}{c}{P}^{1T}X\\ P^{2T}X\\ P^{3T}X\end{array}\right]=0$$
即：$$\begin{gathered} x(P^{3T}X)-P^{1T}X=0 \\ y(P^{3T}X)-P^{2T}X=0 \\ x(P^{2T}X)-y(P^{1T}X)=0 \end{gathered}$$
由于第三个公式可以有上两个公式可得，因此有：
$$\begin{gathered} x_1(P_1^{3T}X)-P_1^{1T}X=0 \\ {y}_1(P_1^{3T}X)-P_1^{2T}X=0 \\ {x}_2(P_2^{3T}X)-P_2^{1T}X=0 \\ {y}_2(P_2^{3T}X)-P_2^{2T}X=0 \end{gathered}$$
可以化简为$$AX=0$$
从而可以求得$X$的3D坐标.

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);
}

两种求解方法后者精度高些。

方法二

---

已知： 两个相机的相对位姿$T$的情况下，得到在两个视图下的对应匹配点$x\leftrightarrow x\prime$
求得: 图像点的深度
解法： 以第一个图像帧为参考帧，R和t是第i到第i+1帧的旋转和位移

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;
}

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;
}

待完善
参考链接