ORB-SLAM3代码详解----基本框架解析及ORB特征提取

一、典型的视觉SLAM系统

     一个典型的视觉SLAM系统主要包含数据处理、初始化、视觉里程计、地图维护、闭环检测等部分。

1.1 关键技术解析

1.1.1 初始化关键技术解析

初始化：主要就是以第一帧图像为参考帧，对后续的视觉帧进行特征匹配、位姿求解（对极约束）、三角化完成3D地图点的生成。供后续pnp（3D-2D）求解。ORB初始化时并行计算单应矩阵和基础矩阵，根据模型得分自动选择选用何种模型进行求解。

1）对极约束----解决的问题是根据两张图像上的相匹配的点，计算出相机的运动。即求解R,T

对极约束求相机运动的步骤：

1、提取ORB特征，匹配点对

2、根据匹配点对计算基础矩阵 再求解到本质矩阵E，通过本质矩阵得到相机位姿 R，t；

基础矩阵的求解方法：

1，直接线性变换法（8点法+最小二乘法）

2，RANSAC-估计基础矩阵

注意：当基础矩阵F为零矩阵，这时候需要使用单应矩阵H，场景中的点都在同一个平面上，可以使用单应矩阵计算像点的匹配点。 相机的平移距离相对于场景的深度较小的时候，也可以使用单应矩阵H。

总流程的代码可以参考：https://blog.csdn.net/floatinglong/article/details/116279786

使用估计的 R，t 可以验证各个特征点对的对极约束误差（理论上误差精确为0），验证的方法为：

//2、估计相机运动
    Mat R, t;
    estimateMoving(kp1, kp2, matches, R, t);
 
    //3、验证本质矩阵的尺度不变性
    Mat tx = (    Mat_<double>(3, 3) << 
        0                    , -t.at<double>(2, 0)    , t.at<double>(1, 0)    ,
        t.at<double>(2, 0)    , 0                        , -t.at<double>(0, 0)    ,
        -t.at<double>(1, 0)    , t.at<double>(0, 0)    , 0                        );    //t映射的反对称矩阵
    Mat E(tx*R);
    cout << "E : \n" << E << endl;    //tx*R 与前面输出的 E ，各元素之间相差一个非零常数
 
    //4、验证对极约束    
    Mat k = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    for(int i = 0; i < (int)matches.size(); i++)
    {
        //取匹配点对的像素坐标
        Point2d p1(kp1[matches[i].queryIdx].pt.x, kp1[matches[i].queryIdx].pt.y);
        Point2d p2(kp2[matches[i].trainIdx].pt.x, kp2[matches[i].trainIdx].pt.y);
        
        //计算归一化坐标的(x, y)
        Point2d y1 = pixelToNor(p1, k);
        Point2d y2 = pixelToNor(p2, k);
        
        //构造归一化坐标
        Mat x1 = (Mat_<double>(3, 1) << y1.x, y1.y, 1);
        Mat x2 = (Mat_<double>(3, 1) << y2.x, y2.y, 1);
        
        printf("对第%03d个特征点的对极约束误差为：", i);
        cout << x2.t() * E * x1 << endl;
    }

** 对极几何（Epipolar Geometry）**描述的是两幅视图之间的内在射影关系，与外部场景无关，只依赖于摄像机内参数和这两幅视图之间的相对位姿。对该技术的讲解可以参考以下博文：

基础矩阵，本质矩阵，单应性矩阵讲解_Being_young的博客-CSDN博客ORB-SLAM点云地图中相机的位姿初始化，无论算法工作在平面场景，还是非平面场景下，都能够完成初始化的工作。其中主要是使用了适用于平面场景的单应性矩阵H和适用于非平面场景的基础矩阵F，...https://blog.csdn.net/u013019296/article/details/108820110

对极几何的图示：

 两视图的对极几何可以理解为图像平面与以基线为轴的平面束相交的几何关系，其中主要有几种概念：

（1）基线（base line）:两个相机中心的连线CC'称为基线。

（2）对极点（epipolar）:ee'是对极点，是基线与两个成像平面的交点，也就是两个相机在另一个成像平面上的像点。

（3）对极平面（epipolar plane）:过基线的平面都称之为对极平面，其中两个相机的中心C和C',三维点X，以及三维点在两个相机成像点xx'这五点必定在同一对极平面上，当三维点X变化时，对极平面绕着基线旋转，形成对极平面束。

（4）对极线（epipolar line）：是对极平面和成像平面的交线，所有的对极线都相交于极点。

对极约束是指在平面2上的p点在平面1上的对应点一定在基线I'上，这句话说明了对极约束是一个点到直线的射影映射关系。如图所示：

算法原理：空间的一个点P。在平面上的投影点，一定在两个像平面的极线上，且具有一定的旋转平移关系，通过对极平面的约束，然后求解出相机的运动，即R，t.

ORB-SLAM中通过E、F矩阵就可以利用两视图中的匹配点求解出相对姿态了，不过这个方法存在一个问题——当两个视图的相机中心相同时，也就是R，t中的t为0，这时对极几何的基础也就不成立了，可知E、F均为0无法求解。这时就需要使用平面间的单应性H矩阵恢复R，t。

单应性矩阵Homogeneous是射影几何中的一个术语，又称之为射影变换。本质上是一个数学概念，一般所说的单应矩阵是平面上的单应性矩阵，主要用来解决两个问题：

（1）表述真实世界中一个平面与他对应图像的透视变换

（2）通过透视变换实现图像从一个视图变换到另一个视图的转换。

单应矩阵求解方法：

（1）直接线性变换法。

（2）RANSAC-估计单应矩阵

平面的单应矩阵和对极约束的F矩阵的区别

两图像间的对极约束和场景的结构无关，可以理解对极约束对于任意场景结构的两幅图像都是成立的，约束是不能给出两幅图像上的像点的一一对应关系，但是可以给出点对应的必要条件，另一幅图像上对应的像点位于对应的对极线上。基础矩阵F描述的实际是一种点和直线的映射关系，而不是一种点对点的约束关系，并不能给出另一个点的确切位置。

平面间的单应矩阵，并不像对极约束完全不需要场景的结构信息，它对场景的结构有了要求，场景的点必须在同一个平面上，因此单应矩阵H也就能够对两图像上对应点的提供更多的约束，知道了某点在一幅图像的像点位置后，可以通过单应矩阵，求得其在另一幅图像中像点的确切位置。

单应矩阵的应用场景是相机只有旋转而无平移的时候，两视图的对极约束不成立，基础矩阵F为零矩阵，这时候需要使用单应矩阵H，场景中的点都在同一个平面上，可以使用单应矩阵计算像点的匹配点。 相机的平移距离相对于场景的深度较小的时候，也可以使用单应矩阵H。

2）三角测量

利用对极几何约束估计相机运动之后，我们还需要通过三角测量来估计地图点的深度,三角测量（三角化）指的是，通过在两处观测同一个点的夹角，从而确定该点的距离。SLAM中主要用三角化来估计像素点的距离。

二、ORB-SLAM3结构解析

输入【Frame and IMU】：frame可以是单目，双目和RGB-D,外加一个IMU，视觉出来的图像，主要是使用ORB算法进行特征提取，IMU的数据主要是用来做积分。

**Tracking **：和ORB-SLAM2的第一个区别。在跟踪模块Tracking ，之前是只根据图像视觉的算法做的，在ORB-SLAM3中，计入加入了IMU的作用。

   处理传感器信息并实时计算当前帧在**激活地图中的姿态**。同时该模块也决定了是否将当前帧作为关键帧。在视觉-惯性模式下，通过在优化中加入惯性残差来估计刚体速度和 IMU 偏差。当追踪丢失时，tracking 线程会尝试在 Atlas 地 图中重定位当前帧。若重定位成功，恢复追踪，并在需要的时候切换激活地图。**若一段时间后仍未激活成功，该激活地图会被存储为未激活地图，并重新初始化一个新的激活地图。所以在ORBSLAM3中的地图池中会有激活地图集和非激活地图池。**

Local mapping 模块：加入关键帧和地图点到当前激活地图，删除冗余帧，并通过对当前帧的附近关键帧操作，利用视觉 BA 或视觉-惯性 BA 技术来优化地图。此外，在惯性模式下，mapping 线程会利用最大后验估计(MAP)技术来初始化和优化 IMU 参数。

Loop and map merging 模块：每当加入一个新的关键帧，该线程在激活地图和整个 Atlas 地图中检测公共区域。如果该公共区域属于激活地图，它就会执行回环校正；如果该公共区域属 于其他地图，就会把它们融合为一个地图，并把这个融合地图作为新的激活地图。在回环校正以后，一个独立线程就会进行全局 BA，进一步优化地图，同时并不影 响实时性能。ORB-SLAM2该模块结构如下图中的右边的图所示。

Atlas【地图集】 模块:一个由一系列离散地图组成的多子图系统。分为活跃地图和非活跃地图，当前帧当做活跃地图。如果跟着跟着跟丢了，就将前面的活跃地图转换为非活跃地图。它会维护一个激活地图，用于tracking 线程对当前帧的定位，同时 local mapping 线程会利用新的关键帧信 息持续对该地图优化和更新。在 Atlas 中的其它地图被称为未激活地图。该系统基于词袋模型对关键帧信息建立数据库，用于重定位、闭环检测和地图融合。这里也是一个提升点，ORB-SLAM2该模块如下图中的右边的图所示。

总结：即使在纯视觉下，ORBSLAM3的性能比ORBSLAM2的性能也是提升了很多

以下是不同的模块，使用的匹配算法的总结：

ORB-SLAM2和ORB-SLAM3之间的对比

三、代码流程图

三、ORBSLAM3源码解读

3.1 ORB特征提取

ORB_SLAM3 算法框架解析_墙头玩飞车的博客-CSDN博客_orbslam3框架