三角形法恢复空间点深度

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

1.求解空间点坐标

当我们得到两个视图的一组匹配点，我们希望能恢复出世界点在三维世界的坐标。这里就涉及到使用三角形法来恢复点在3D空间的结构。一般比较常用的方法是线性三角形法（Linear triangulation methods ）。线性三角形法使用直接线性变化（DLT）对点的世界坐标进行求解。
已知点对和和两个图像的投影矩阵和，根据相机投影模型，对应3D点满足：
$\begin{cases} \mathbf x_1 = P_1 \mathbf X \\ \mathbf x_2= P_2\mathbf X \end{cases}$
这里 $x_1$ 、 $x_2$ 是归一化后特征点坐标， $X$ 为三维空间点在世界坐标系的齐次坐标 $X=\begin{matrix} [x & y &z &1]^T \end{matrix}$ 使用DLT需要把式子改变成的形式。由于是齐次坐标的表示形式，使用叉乘消去齐次因子，有
$\begin{cases} \mathbf x_1 \times (P_1 \mathbf X) = \mathbf 0 \\ \mathbf x_2 \times (P_2\mathbf X)=\mathbf 0 \end{cases}$
把和按照行展开代入，对第一幅图 $I_1$ 有
$\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$
即
$\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}$
由此可以得到三个方程，由于第三个方程可以由前两个方程得到(第三个方程可由前两个方程线性表示)，因此只需要考虑前两个方程。每对匹配的特征（ $x_1$ 和 $x_2$ ）都会得到四个方程，表示为 $A X = 0$ 的形式：
$=\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}$
由于是自由度为3的齐次方程，所以这是一个冗余的方程，这里相当于解一个线性最小二乘问题。方程的解为的最小奇异值对应的单位奇异矢量，解得，则最后令缩放使得的最后一项为1即可得到我们所求的3D点的坐标。
VINS-Mono 中的三角形法的实现代码如下：

/*** @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中的三角形法的实现代码如下：

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