三角形法恢复空间点深度

三角形法恢复空间点深度

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

1.求解空间点坐标

当我们得到两个视图的一组匹配点，我们希望能恢复出世界点在三维世界的坐标。这里就涉及到使用三角形法来恢复点在3D空间的结构。一般比较常用的方法是线性三角形法（Linear triangulation methods ）。线性三角形法使用直接线性变化（DLT）对点的世界坐标进行求解。
已知点对和和两个图像的投影矩阵和 ，根据相机投影模型，对应3D点满足 ：
$$\{\begin{array}{l}{\mathbf{x}}_1=P_1\mathbf{X}\\ {\mathbf{x}}_2=P_2\mathbf{X}\end{array}$$
这里$x_1$、$x_2$是归一化后特征点坐标，$X$为三维空间点在世界坐标系的齐次坐标$X=\begin{array}{cccc}[x& y& z& 1{]}^T\end{array}$使用DLT需要把式子改变成的形式。由于是齐次坐标的表示形式，使用叉乘消去齐次因子，有
$$\{\begin{array}{l}{\mathbf{x}}_1\times (P_1\mathbf{X})=0\\ {\mathbf{x}}_2\times (P_2\mathbf{X})=0\end{array}$$
把和按照行展开代入，对第一幅图$I_1$有
$$\left[\begin{array}{ccc}0& -1& y_1\\ 1& 0& -x_1\\ -y_1& x_1& 0\end{array}\right]\left[\begin{array}{c}{P}_1^{1T}X\\ P_1^{2T}X\\ P_1^{3T}X\end{array}\right]=0$$
即
$$\begin{array}{r}{x}_1(P_1^{3T}X)-(P_1^{1T}X)=0\\ y_1(P_1^{3T}X)-(P_1^{2T}X)=0\\ x_1(P_1^{2T}X)-y_1(P_1^{1T}X)=0\end{array}$$
由此可以得到三个方程，由于第三个方程可以由前两个方程得到(第三个方程可由前两个方程线性表示)，因此只需要考虑前两个方程。每对匹配的特征（$x_1$和$x_2$）都会得到四个方程，表示为$AX=0$ 的形式：
$$A=\left[\begin{array}{c}{x}_1{P}_1^{3T}-P_1^{1T}\\ y_1{P}_1^{3T}-P_1^{2T}\\ x_2{P}_2^{3T}-P_2^{1T}\\ y_2{P}_2^{3T}-P_2^{2T}\end{array}\right]$$
由于是自由度为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);
}