orb-slam2学习总结

视觉SLAM

1、地图初始化

2、ORB_SLAM地图初始化流程

3、ORB特征提取及匹配

1、对极几何

2、对极约束（epipolar constraint）

3、基础矩阵F、本质矩阵E

5、单目尺度不确定性

6、单应矩阵（Homography Matrix）

6.1 什么是单应矩阵

6.2 H矩阵求解

7.1 基础要点

7.2 从E矩阵中恢复R、t

7.3 从H矩阵中恢复R、t

7.4 R、t值验证

7、三角化

7.1什么是三角化

7.2 三角化求解

8、关键步骤总结

9、参考资料

视觉SLAM

一、知识点

1、地图初始化

视觉slam中，地图初始化是一个非常重要的步骤，它决定了后续的跟踪、建图等模块的效果。简单来说，初始化的目的是利用前几帧图像，计算出相机位姿，并且构建出第一批3D地图点。第一批3D地图点为跟踪、建图等模块提供了一个初始的地图，这样后续的模块就可以利用这些地图点进行跟踪、建图等工作。

双目由于已知两个相机的内外参，可以直接三角化出3D点。三角化是单目的一个步骤，所以双目初始化比较简单。

RGBD可以直接通过深度值还原3D坐标，更加简单。

配置了IMU之后实际也可以直接获得两个相机之间的姿态，也可以直接三角化出3D点（虽然IMU初始化需要考虑IMU参数的初始化，但是这部分内容不在本文的讨论范围内）。

2、ORB_SLAM地图初始化流程

3、ORB特征提取及匹配

特征点提取是一个比较独立的内容，对于ORB特征提取可以参考这篇文章：https://zeal-up.github.io/2023/05/18/orbslam/orbslam3-ORBextractor/

备注：根据描述子之间的距离来寻找两帧间距离最近的关键件，达到关键点匹配的效果。

对极几何及对极约束

1、对极几何

通常，我们会将第一帧图像当作参考帧，也就是世界坐标系。第二帧相机的位姿也就是相对于第一帧相机的位姿。

总结：

利用两相机在空间中成像的（空间几何关系，也叫立体几何）规律，进而求解此时相机的位姿态。因此就需要用到特征提取步骤提取到得匹配点，利用匹配点利用对极约束求解相机位姿。

那么什么是对极约束呢？

2、对极约束（epipolar constraint）

3、基础矩阵F、本质矩阵E

Foundamental Matrix、Essential Matrix

对极约束的几何意义

备注：

当需要寻找关键点在另外一张图片上投影点时候，评估相机姿态质量的时候，同样道理。

又或者在极线方向上寻找匹配点，避免全图片检索，提到检索效率。这些都是对极约束的应用。

本质矩阵和基础矩阵求解

方程求解

opencv求解

5、单目尺度不确定性

在看对极几何的图

总结：

尺度信息在等式中无法接着求解，只能利用别的设备增加深度信息才能更好求解尺度。

6、单应矩阵（Homography Matrix）

6.1 什么是单应矩阵

前面讨论基础矩阵的概念以及如何从一些匹配点对关系中求解基础矩阵。我们没有对关键点是否在一个平面上进行任何假设。但是，如果我们假设关键点在一个平面上，那么我们就可以使用单应矩阵（Homography Matrix）来求解相机之间的位姿。当匹配点对的关键点都是在3D空间中一个平面上时，这些点对关系可以通过单应矩阵来描述（相差一个常量系数）

备注：要求匹配点对应点对是3D空间，同一平面上的。

如下图：

6.2 单应矩阵的应用

相机位姿态求解
图像拼接

链接：

超详细！从单应矩阵推导到自动驾驶环视投影应用 | Zeal's Blog

6.2 H矩阵求解

求解推导

OpenCV接口

位姿求解

如何从H、E矩阵恢复R、t？

7.1 基础要点

7.2 从E矩阵中恢复R、t

可使用opencv接口

7.3 从H矩阵中恢复R、t

从H矩阵恢复R、t有多种方法，论文中的方法叫Faugeras SVD-based decomposition，最终可以求解出8种解。另外一种有名的数值解法（通过奇异值分解）叫

SVD-based decomposition

OpenCV的接口使用的是分析解法：https://inria.hal.science/inria-00174036v3/documentOpenCV的接口最终返回4种解

OpenCV的接口使用说明：cv::decomposeHomographyMat

7.4 R、t值验证

从E、H分解出来的矩阵后，需要选择出正确的R、t

无论从E矩阵还是H矩阵中恢复出R、t，都会得到多种解。

我们需要从这些解中选择出正确的解。最简单的做法是利用分解出的R、t对匹配点进行三角化，并检查该3D点在两个相机下的深度值，3D点必须在两个相机下都是正的才是正确的解。

对于单应矩阵的分解结果，OpenCV提供了一个函数可以帮助我们选择正确的解：cv::filterHomographyDecompByVisibleRefpoints

在ORB_SLAM中，对于每一种解，都会对所有匹配点进行三角化，对三角化出来的点，会进行很多步骤的检查，最后选择拥有最多内点的解作为正确的解。

备注：什么是内点？

7、三角化

7.1什么是三角化

当求解出位姿态后，需要利用位姿，联合匹配点关系，求解出三维点坐标。

7.2 三角化求解

用SVD求解上述方程，求解出的3D坐标有4个元素，需要将第四个元素归一化为1。这里把ORB_SLAM的这部分代码也贴出来。

/** 
 * @brief 三角化获得三维点
 * @param x_c1 点在关键帧1下的归一化坐标
 * @param x_c2 点在关键帧2下的归一化坐标
 * @param Tc1w 关键帧1投影矩阵  [K*R | K*t]
 * @param Tc2w 关键帧2投影矩阵  [K*R | K*t]
 * @param x3D 三维点坐标，作为结果输出
 */
bool GeometricTools::Triangulate(
    Eigen::Vector3f &x_c1, Eigen::Vector3f &x_c2, Eigen::Matrix<float,3,4> &Tc1w, Eigen::Matrix<float,3,4> &Tc2w,
    Eigen::Vector3f &x3D)
{
    Eigen::Matrix4f A;
    // x = a*P*X， 左右两面乘x的反对称矩阵 a*[x]^ * P * X = 0 ，[x]^*P构成了A矩阵，中间涉及一个尺度a，因为都是归一化平面，但右面是0所以直接可以约掉不影响最后的尺度
    //  0 -1 v    P(0)     -P.row(1) + v*P.row(2)
    //  1 0 -u *  P(1)  =   P.row(0) - u*P.row(2) 
    // -v u  0    P(2)    u*P.row(1) - v*P.row(0)
    // 发现上述矩阵线性相关(第一行乘以-u加上第二行乘以-v等于第三行），所以取前两维，两个点构成了4行的矩阵（X分别投影到相机1和相机2），就是如下的操作，求出的是4维的结果[X,Y,Z,A]，所以需要除以最后一维使之为1，就成了[X,Y,Z,1]这种齐次形式
    A.block<1,4>(0,0) = x_c1(0) * Tc1w.block<1,4>(2,0) - Tc1w.block<1,4>(0,0);
    A.block<1,4>(1,0) = x_c1(1) * Tc1w.block<1,4>(2,0) - Tc1w.block<1,4>(1,0);
    A.block<1,4>(2,0) = x_c2(0) * Tc2w.block<1,4>(2,0) - Tc2w.block<1,4>(0,0);
    A.block<1,4>(3,0) = x_c2(1) * Tc2w.block<1,4>(2,0) - Tc2w.block<1,4>(1,0);

    // 解方程 AX=0
    Eigen::JacobiSVD<Eigen::Matrix4f> svd(A, Eigen::ComputeFullV);

    Eigen::Vector4f x3Dh = svd.matrixV().col(3);

    if(x3Dh(3)==0)
        return false;

    // Euclidean coordinates
    x3D = x3Dh.head(3)/x3Dh(3);

    return true;
}