三角形法恢复空间点深度三角形法恢复空间点深度

115 阅读 0 评论 76 点赞

我是靠谱客的博主霸气指甲油，最近开发中收集的这篇文章主要介绍三角形法恢复空间点深度三角形法恢复空间点深度，觉得挺不错的，现在分享给大家，希望可以做个参考。

概述

三角形法恢复空间点深度

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

1.求解空间点坐标

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

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

x = P X x' = P' X (1) (1) x = P X x ' = P ' X

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

x \times (P X) = 0 x' \times (P' X) = 0 (2) (2) x \times ( P X ) = 0 x ' \times ( P ' X ) = 0

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

⎡ ⎣ ⎢ 01 - y - 1 0 x y - x 0 ⎤ ⎦ ⎥ ⎡ ⎣ ⎢ P 1 T X P 2 T X P 3 T X ⎤ ⎦ ⎥ = 0 (3) (3) [ 0 - 1 y 1 0 - x - y x 0 ] [ P 1 T X P 2 T X P 3 T X ] = 0

即

x (P 3 T X) - (P 1 T X) = 0 y (P 3 T X) - (P 2 T X) = 0 x (P 2 T X) - y (P 1 T X) = 0 (13) (13) x ( P 3 T X ) - ( P 1 T X ) = 0 y ( P 3 T X ) - ( P 2 T X ) = 0 x ( P 2 T X ) - y ( P 1 T X ) = 0

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

A = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ x P 3 T - P 1 T y P 3 T - P 2 T x' P' 3 T - P' 1 T y' P' 3 T - P' 2 T ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ (30) (30) A = [ x P 3 T - P 1 T y P 3 T - P 2 T x ' P ' 3 T - P ' 1 T y ' P ' 3 T - P ' 2 T ]

由于 X X 是自由度为3的齐次方程，所以这是一个冗余的方程，这里相当于解一个线性最小二乘问题。方程的解为 A A 的最小奇异值对应的单位奇异矢量，解得 $x = (x, y, z, w)$ ，则最后令 X 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 的图像帧设置为参考帧，在其他的图像帧中上做对极线，在对极线上搜索匹配点 x′ x ′ ，然后通过匹配点进行三角法恢复深度。我们设相机内参为 K K ，则有：

\begin{matrix} (6) & f = K^{- 1} x f^{'} = K^{- 1} x^{'} \end{matrix}

这里的

f f 和

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

z' \cdot f' = [R | t] z \cdot f = z \cdot R f + t (7) (7) z ' \cdot f ' = [ R | t ] z \cdot f = z \cdot R f + t

这里的

z z 和

z^{'}

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

(- R f f') (z z') = t (8) (8) ( - R f f ' ) ( z z ' ) = t

这里就是

Ax=b A x = b 的形式，要解该方程，可以用正规方程来求解，也就是解

AT(Ax−b)=0 A T ( A x − b ) = 0 ，也就是有：

A T A x = A T b (9) (9) A T A x = A T b

所以解得:

x = (A T A) - 1 A T b (10) (10) x = ( A T A ) - 1 A T b

在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 x 是二维的，其实在得到公式 (9) ( 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

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