0. 简介

三角化作为SLAM中的基础问题，最近经常被提及，通过单目运动的方式可以有效的恢复深度信息

1. 线性三角化数学推导

特征点在某个相机中被观测到，根据相机位姿和观测向量可以得到3D空间中的一条从相机中心出发的观测射线，多个相机位姿观测会产生多条观测射线，理想情况下这些观测射线会相交于空间中的一点，求所有观测射线的交点就是特征点在3D空间的位置，这也就是三角化过程。
在这里插入图片描述
$x$,$x’$为匹配特征点对$\begin{bmatrix} u \ v \ 1 \end{bmatrix}$，$P$,$P’$为投影矩阵$[R,t]$，$X$为空间中的三维点$\begin{bmatrix} x \ y \ z \ 1 \end{bmatrix}$,$\mu$为需要求解的深度值

$x = 1/\mu _ P _ X$

$x’ = 1/\mu _ P’ _ X$

此时式子1可以得到：

${\mu}x{\times}PX$= 0

并根据矩阵差乘推出正交投影矩阵,因为${\mu}$为常数，所以去除。

\hat{x}P X= 0

根据上式展开：

\hat{x}P X = \begin{bmatrix} 0 && -1 && v \ 1 && 0 && -u \ -v && u && 0 \end{bmatrix}\begin{bmatrix} {r_1} \ {r_2} \ {r_3} \end{bmatrix}X
= \begin{bmatrix} -{r_2} +v{r_3}\ {r_1}-u{r_3} \ -v{r_1}+u{r_2} \end{bmatrix}X =0

其中函数\begin{bmatrix} -{r_2} +v{r_3}\ {r_1}-u{r_3} \ -v{r_1}+u{r_2} \end{bmatrix}中的第一行和第二行与第三行线性相关（通过第一行$$_-u$，第二行$_-v$$，再相加）。所以可以等效为：

\begin{bmatrix} -{r_2} +v{r_3}\ {r_1}-u{r_3} \end{bmatrix}X = 0

此时的矩阵是一个匹配特征点x和一个投影矩阵P组成，而我们可以获得该X三维点的多个观测，此时可以根据多个观测构建出：
\begin{bmatrix} -{r_2} +v{r_3}\ {r_1}-u{r_3} \-{r_2’} +v’{r_3’}\ {r_1’}-u’{r_3’}\ …. \end{bmatrix}X = 0

此时该方程为满秩r(A) = n的，所以不存在非零解，此时使用SVD求解是惟一的
在这里插入图片描述
然后通过归一化保证结果为齐次坐标的
X= \begin{bmatrix} x \ y \ z \ 1 \end{bmatrix} = \begin{bmatrix} x_0/x_3 \ x_1/x_3 \ x_2/x_3 \ x_3/x_3 \end{bmatrix}

2. 常见的SLAM中的三角化代码

ORB-SLAM2


/*
 * 三角化得到3D点 
 *  *三角测量法 求解 两组单目相机  图像点深度
 * s1 * x1 = s2  * R * x2 + t
 * x1 x2 为两帧图像上 两点对 在归一化坐标平面上的坐标 k逆* p
 * s1  和 s2为两个特征点的深度 ，由于误差存在， s1 * x1 = s2  * R * x2 + t不精确相等
 * 常见的是求解最小二乘解，而不是零解
 *  s1 * x1叉乘x1 = s2 * x1叉乘* R * x2 + x1叉乘 t=0 可以求得x2
 * 
 */
/*
 平面二维点摄影矩阵到三维点  P1 = K × [I 0]    P2 = K * [R  t]
  kp1 = P1 * p3dC1       p3dC1  特征点匹配对 对应的 世界3维点
  kp2 = P2 * p3dC1  
  kp1 叉乘  P1 * p3dC1 =0
  kp2 叉乘  P2 * p3dC1 =0  
 p = ( x,y,1)
 其叉乘矩阵为
     //  叉乘矩阵 = [0  -1  y;
    //                       1   0  -x; 
    //                      -y   x  0 ]  
  一个方程得到两个约束
  对于第一行 0  -1  y; 会与P的三行分别相乘 得到四个值 与齐次3d点坐标相乘得到 0
  有 (y * P.row(2) - P.row(1) ) * D =0
      (-x *P.row(2) + P.row(0) ) * D =0 ===> (x *P.row(2) - P.row(0) ) * D =0
    两个方程得到 4个约束
    A × D = 0
    对A进行奇异值分解 求解线性方程 得到 D  （D是3维齐次坐标，需要除以第四个尺度因子 归一化）
 */
// Trianularization: 已知匹配特征点对{x x'} 和 各自相机矩阵{P P'}, 估计三维点 X
// x' = P'X  x = PX
// 它们都属于 x = aPX模型
//                                             |X|
// |x|       |p1 p2   p3  p4   ||Y|          |x|      |--p0--||.|
// |y| = a |p5 p6   p7  p8   ||Z| ===>|y| = a|--p1--||X|
// |z|       |p9 p10 p11 p12||1|          |z|      |--p2--||.|
// 采用DLT的方法：x叉乘PX = 0
// |yp2 -  p1|        |0|
// |p0  -  xp2| X = |0|
// |xp1 - yp0|       |0|
// 两个点:
// |yp2   -  p1  |       |0|
// |p0    -  xp2 | X = |0| ===> AX = 0
// |y'p2' -  p1' |        |0|
// |p0'   - x'p2'|        |0|
// 变成程序中的形式：
// |xp2  - p0 |       |0|
// |yp2  - p1 | X = |0| ===> AX = 0
// |x'p2'- p0'|        |0|
// |y'p2'- p1'|        |0|
/**
 * @brief 给定投影矩阵P1,P2和图像上的点kp1,kp2，从而恢复3D坐标
 *
 * @param kp1 特征点, in reference frame
 * @param kp2 特征点, in current frame
 * @param P1  投影矩阵P1
 * @param P2  投影矩阵P２
 * @param x3D 三维点
 * @see       Multiple View Geometry in Computer Vision - 12.2 Linear triangulation methods p312
 */
    void Initializer::Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D)
    {
      // 在DecomposeE函数和ReconstructH函数中对t有归一化
      // 这里三角化过程中恢复的3D点深度取决于 t 的尺度，
      // 但是这里恢复的3D点并没有决定单目整个SLAM过程的尺度
      // 因为CreateInitialMapMonocular函数对3D点深度会缩放，然后反过来对 t 有改变
        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);//  转换成非齐次坐标  归一化
    }

VINS-Mono

void FeatureManager::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,
                        Eigen::Vector2d &point0, Eigen::Vector2d &point1, Eigen::Vector3d &point_3d)
{
    Eigen::Matrix4d design_matrix = Eigen::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);
    Eigen::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);
}

3. 参考链接

https://blog.csdn.net/xiaoxiaowenqiang/article/details/79278884

https://blog.51cto.com/u_15061934/4266539

https://zhuanlan.zhihu.com/p/387575538

https://blog.csdn.net/michaelhan3/article/details/89483148