Vins-mono 三角形法恢复空间点深度

一、概述

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

二、原理

图1
根据几何关系，假设两帧图像上匹配特征点的齐次坐标为x1, x2。三维空间点齐次坐标记为X。两帧图像对应的投影矩阵为P1，P2。可以得到：

x1 = P1 X
x2 = P2X

使用叉乘得到AX = 0的形式（可以使用SVD的方法计算）：

x1×(P1 X) = 0
x2×(P2 X) = 0

其中对X = (x, y, z)分析，将P1和P2按行拆开，得到：

由此可以得到三个方程，由于第三个方程可以由前两个方程得到，因此只需要考虑前两个方程。每对匹配的特征（x1和x2）都会得到四个方程，表示为AX =0 的形式，

通常由于匹配的特征点对较多，这是一个超定方程，一般无法求得精确解，我们其实需要求一个使AX最小的解。通过对A的SVD分解，最后得到的是A的最小奇异值对应的单位奇异矢量，若X = (x, y, z, w)，深度则为z/w。

三、代码

void GlobalSFM::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,
						Vector2d &point0, Vector2d &point1, Vector3d &point_3d)
{
	// 通过奇异值分解求解一个Ax = 0得到，对应上述推导
	Matrix4d design_matrix = Matrix4d::Zero();
	design_matrix.row(0) = point0[0] * Pose0.row(2) - Pose0.row(0);
	design_matrix.row(1) = point0[1] * Pose0.row(2) - Pose0.row(1);
	design_matrix.row(2) = point1[0] * Pose1.row(2) - Pose1.row(0);
	design_matrix.row(3) = point1[1] * Pose1.row(2) - Pose1.row(1);
	Vector4d triangulated_point;
	triangulated_point =
		      design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>();
	// 齐次向量归一化
	point_3d(0) = triangulated_point(0) / triangulated_point(3);
	point_3d(1) = triangulated_point(1) / triangulated_point(3);
	point_3d(2) = triangulated_point(2) / triangulated_point(3);
}