三角形法恢复空间点深度

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

1.求解空间点坐标

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

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

x=PXx′=P′X

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

x×(PX)=0x′×(P′X)=0

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

⎡⎣⎢01−y−10xy−x0⎤⎦⎥⎡⎣⎢P1TXP2TXP3TX⎤⎦⎥=0

即

x(P3TX)−(P1T)=0y(P3TX)−(P2T)=0x(P2TX)−y(P1T)=0

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

A=⎡⎣⎢⎢⎢⎢x(P3TX)−(P1T)y(P3TX)−(P2T)x′(P′3TX)−(P′1T)y′(P′3TX)−(P′2T)⎤⎦⎥⎥⎥⎥

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

ORB-SLAM21中的三角形法的代码如下：

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.求解空间点深度

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

f=K−1xf′=K−1x′

这里的f和f′是单位平面上的点。通过点的对应关系，我们有：

z′⋅f′=[R|t]z⋅f=z⋅Rf+t

这里的z和z′分别对应空间点在两个相机坐标系下的深度。于是我们可以把公式化为：

(−Rff′)(zz′)=t

这里就是Ax=b的形式，要解该方程，可以用正规方程来求解，也就是解AT(Ax−b)=0，也就是有：

ATAx=ATb

所以解得:

x=(ATA)−1ATb

在SVO2中的实现如下，返回在参考帧下点的深度：

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

由于解向量x是二维的，其实在得到公式(9)之后，我们也可以直接采用克莱默法则求解。在REMODE3和《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

---

1. https://github.com/raulmur/ORB_SLAM2/blob/master/src/Initializer.cc#L734 ↩
2. https://github.com/uzh-rpg/rpg_svo/blob/master/svo/src/matcher.cpp#L109 ↩
3. https://github.com/uzh-rpg/rpg_open_remode/blob/master/src/triangulation.cu#L30 ↩
4. https://github.com/gaoxiang12/slambook/blob/master/ch13/dense_monocular/dense_mapping.cpp#L348 ↩