三角形法恢复空间点深度

如下图，以图 I 1 I_1 I1为参考，图 I 2 I_2 I2的变换矩阵为 T T T。相机光心为 O 1 O_1 O1和 O 2 O_2 O2。在图 I 1 I_1 I1中有特征点 p 1 p_1 p1，对应图 I 2 I_2 I2中有特征点 p 2 p_2 p2。理论上直线 O 1 p 1 O_1p_1 O1p1 与 O 2 p 2 O_2p_2 O2p2 在场景中会相交于一点 P P P，该点即是两个特征点所对应的地图点在三维场景中的位置。(由于噪声的影响，这两条直线往往无法相交)。简言之，在已知两个相机的相对位姿的情况下，得到在两个视图下的对应匹配点，即可求得该对应点在空间中的位置，也就是求得图像点的深度
参考链接1 、参考链接2 、论文链接

1.求解空间点坐标

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

已知点对和和两个图像的投影矩阵和，根据相机投影模型，对应3D点满足：
{ x 1 = P 1 X x 2 = P 2 X \begin{cases} \mathbf x_1 = P_1 \mathbf X \\ \mathbf x_2= P_2\mathbf X \end{cases} {x1=P1Xx2=P2X

这里 x 1 x_1 x1、 x 2 x_2 x2是归一化后特征点坐标， X X X为三维空间点在世界坐标系的齐次坐标 X = [ x y z 1 ] T X=\begin{matrix} [x & y &z &1]^T \end{matrix} X=[xyz1]T使用DLT需要把式子改变成的形式。由于是齐次坐标的表示形式，使用叉乘消去齐次因子 ，有
{ x 1 × ( P 1 X ) = 0 x 2 × ( P 2 X ) = 0 \begin{cases} \mathbf x_1 \times (P_1 \mathbf X) = \mathbf 0 \\ \mathbf x_2 \times (P_2\mathbf X)=\mathbf 0 \end{cases} {x1×(P1X)=0x2×(P2X)=0

把和按照行展开代入，对第一幅图 I 1 I_1 I1有
[ 0 − 1 y 1 1 0 − x 1 − y 1 x 1 0 ] [ P 1 1 T X P 1 2 T X P 1 3 T X ] = 0 \begin{bmatrix} 0 & -1 & y_1\\ 1 & 0 & -x_1\\ -y_1 & x_1 & 0 \end{bmatrix} \begin{bmatrix} P^{1T}_1X\\P^{2T}_1X\\P^{3T}_1X \end{bmatrix} = 0 01−y1−10x1y1−x10 P11TXP12TXP13TX =0

即
x 1 ( P 1 3 T X ) − ( P 1 1 T X ) = 0 y 1 ( P 1 3 T X ) − ( P 1 2 T X ) = 0 x 1 ( P 1 2 T X ) − y 1 ( P 1 1 T X ) = 0 \begin{split} x_1(P^{3T}_1X)-(P^{1T}_1X)=0\\ y_1(P^{3T}_1X)-(P^{2T}_1X)=0\\ x_1(P^{2T}_1X)-y_1(P^{1T}_1X)=0\\ \end{split} x1(P13TX)−(P11TX)=0y1(P13TX)−(P12TX)=0x1(P12TX)−y1(P11TX)=0

由此可以得到三个方程，由于第三个方程可以由前两个方程得到(第三个方程可由前两个方程线性表示)，因此只需要考虑前两个方程。每对匹配的特征（ x 1 x_1 x1和 x 2 x_2 x2）都会得到四个方程，表示为 A X = 0 AX =0 AX=0 的形式：
A = [ x 1 P 1 3 T − P 1 1 T y 1 P 1 3 T − P 1 2 T x 2 P 2 3 T − P 2 1 T y 2 P 2 3 T − P 2 2 T ] A =\begin{bmatrix} x_1P_1^{3T}-P^{1T}_1\\ y_1P_1^{3T}-P_1^{2T}\\ x_2P_2^{3T}-P_2^{1T}\\ y_2P_2^{3T}-P_2^{2T}\\ \end{bmatrix} A= x1P13T−P11Ty1P13T−P12Tx2P23T−P21Ty2P23T−P22T

由于是自由度为3的齐次方程，所以这是一个冗余的方程，这里相当于解一个线性最小二乘问题。方程的解为的最小奇异值对应的单位奇异矢量，解得，则最后令缩放使得的最后一项为1即可得到我们所求的3D点的坐标。
VINS-Mono 中的三角形法的实现代码如下：

cpp 复制代码

/**
 * @description: DLT 三角形法恢复空间点深度
 * @date: 2024/06/20
 * @param[i]: Pose0: 第1帧 pose
 * @param[i]: Pose1: 第2帧 pose
 * @param[i]: point1: 第一帧 uv 坐标
 * @param[i]: point2: 第二帧 uv 坐标
 * @param[o]: point_3d: 三角化得到的三维坐标
**/
void GlobalSFM::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,
						Vector2d &point0, Vector2d &point1, Vector3d &point_3d)
{
	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);
}

ORB-SLAM2中的三角形法的实现代码如下：

cpp 复制代码

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