0.ORB-SLAM3算法知识点总结_orb-slam生成树

1.ORB-SALM特征提取的一般步骤

• 构建图像金字塔(8层)
• 提取Fast特征点并使用OctTree进行均匀化分布
• 利用灰度质心法，计算特征点角度
• 计算带有旋转不变性的rBRIEF描述子

2.fast特征提取

使用的是cv自带的fast角点提取函数cv::FAST(…),只不过这里作者设置了一个严刻的阈值iniThFAST(20)和一个宽松的阈值minThFAST(7)

这里计算的是中心像素与周围一圈像素灰度值差异，小于这个阈值认为灰度相同即不是特征点，大于则认为不同即特征点，对于中心像素周围一圈的像素需要有连续N个特征点满足阈值要求才可以。

3.特征均匀化处理

1.首先计算每层金字塔图像应该保留多少个特征点，这个是根据配置文件设置的总数来的，根据每层金字塔图像面积和所有层金字塔总面积的比值按照比例分配每层金字塔应该提取特征点的数量。

2.其次基于四叉树原理，为每一层图像选择响应最好特征点并保证特征点的均匀分布,基于harris相应值的升级版。

• 开始的时候只有一个node。
• 如果node里面的点数>1，把每个node分成四个node，如果node里面的特征点为空，就不要了，删掉。
• 新分的node的点数>1，就再分裂成4个node。
• 如此，一直分裂。终止条件为：node的总数量> N_a，或者无法再进行分裂。然后从每个node里面选择一个质量最好的FAST点

4.Oriented FAST：ORB特征的旋转不变性

以Fast特征点为圆心选定一块像素区域，计算所有像素的质心，连接圆心和质心得到一个向量，这个向量的角度就是角点的角度。

5.BRIEF描述子计算

BRIEF是一个二进制的描述子，计算和匹配的速度都很快。计算步骤如下：
(1)以关键点为中心，选择一个31×31的像素块
(2)在这个块内按照一定的方法选择N对点，N一般取256,这里的方法一般是提前通过学习选择的效果最好的256个位置
(3)对于点对{A,B},通过比较这两个点的灰度大小确定一个二值结果，0或者1.

(4)这样把256个点对的结果排起来，就形成了BRIEF描述子。
[1 0 1 0 0 ... 1 0 0]

BRIEF的匹配采用汉明距离，非常快，简单说就是看一下不相同的位数有多少个。如下的两个描述子，不同的位数为4。实际上我们选择的点对数为256，那么距离范围就是0~256。
[1 0 1 1 1 0]
[1 1 0 0 1 1]

6.rBRIEF描述子计算

基于BRIEF描述子的特征匹配就是计算特征点周围像素块的灰度值差异，rBRIEF的核心思想在于对像素块的处理上，在原来的基础上增加了旋转的信息，就是当图像发生旋转了，特征点的方向肯定发生旋转。根据上一步计算的oriented fast关键点的方向，将像素块旋转同样的角度，然后再描述子计算和汉明距离计算。

7.ORB-SLAM3中的双单目组合模式

目前双单目组合模式即将两个单目相机进行搭配，通过提前标定得到两个相机的外参信息，在配置文件中有一个参数Tlr即为双单目模式准备的。对于zed,realsense具备双目的相机。

在深度值计算时是通过基线和视差计算得到的，对于双单目模式是通过正常三角化计算得到，因为两个相机之间的距离是已知的，因此可以恢复正常的尺度信息。

8.特征点去畸变的方式

ORB-SLAM系列对于特征点去畸变是直接调的cv中的函数，但是有个问题，在下面公式描述的畸变模型中是已知正确的坐标，然后乘以相关畸变系数得到畸变坐标，但实际上去畸变处理是已知畸变坐标需要恢复正确的坐标。但是在畸变模型中存在2次项，所以去畸变操作求解时会带来一定的困难。

在vins系列提出一种逐渐逼近式的去畸变操作，使用了一种很巧妙的方法。

畸变的程度越靠近图像边缘越高，这点是VINS中去畸变的主要依据思想。

• 已知含有畸变的A’坐标，需要求出发生畸变前的真实坐标A
• 根据畸变公式可以由真实坐标得到包含畸变坐标是比较容易计算的，反之要复杂很多。
• 因此VINS中将已知的A’当做真实的坐标B，通过畸变公式得到畸变后的B’。得到畸变强度（近似表示）BB’，从A’移动BB’，得到更加靠近A的C点。然后以C继续推导出更靠近A点的D。反复迭代，即可得到在接受范围内无限靠近A的点。

9.基于SAD滑窗的双目特征匹配

1. 为左目每个特征点建立带状区域搜索表，限定搜索区域。
   在右目图像为左目特征点进行匹配搜索的时候，不仅仅是在一条横线上搜索，而是在一条横向搜索带上搜索。简而言之，原本每个特征点的纵坐标为1个像素大小，这里把特征点体积放大，假定纵坐标占好几行

2. 对左目相机每个特征点，通过描述子在右目带状搜索区域找到匹配点

3. 通过SAD滑窗得到匹配修正量bestincR
   简单来说就是基于匹配得到的右目图像中特征点的坐标，计算左右目图像相互匹配特征点周围像素块(11×11)的灰度差值，然后移动右目图像中的像素块滑窗，当灰度差最小时会得到一个特征点的偏移量。(有点像直接法)

然后又因为滑窗的步长是1个像素，所以又做了一次抛物线拟合，求抛物线谷底对应的坐标值(极小值即谷底一定在两个较小值中间)，最终得到亚像素级别的坐标精度。

10.SAD滑窗算法原理

SAD(Sum of absolute differences)是一种图像匹配算法。基本思想：差的绝对值之和。此算法常用于图像块匹配，将每个像素对应数值之差的绝对值求和，据此评估两个图像块的相似度。该算法快速、但并不精确，通常用于多级处理的初步筛选。

• 构造一个小窗口,类似于卷积核；
• 用窗口覆盖左边的图像,选择出窗口覆盖区域内的所有像素点；
• 同样用窗口覆盖右边的图像并选择出覆盖区域的像素点；
• 左边覆盖区域减去右边覆盖区域，并求出所有像素点灰度差的绝对值之和；
• 移动右边图像的窗口，重复（3）-（4）的处理（这里有个搜索范围,超过这个范围跳出）；
• 找到这个范围内SAD值最小的窗口,即找到了左图锚点的最佳匹配的像素块。

图像块灰度差异计算方法：SAD,SSD,NCC

11.浅谈网格化处理

这个是为了加速匹配的一个trick，在特征匹配时如果遍历整副图像中的特征点计算描述子距离无疑会浪费很多时间，因此将图像划分了很多网格，然后将网格与特征点绑定(特征点网格注册)。

在进行匹配时，根据特征点在上一帧的网格位置，然后在当前帧相邻近的多个网格中进行查找，加速特征匹配。

12.双目初始化过程

不管是双目还是单目初始化，主要目的就是计算初始位姿并恢复一组3D地图点用于后续pnp计算。

允许双目初始化的条件：

• 纯双目模式：特征点数量大于500
• 双目+IMU模式：特征点数量大于500 + 当前帧和上一帧都要具备IMU预积分信息(共需要3帧) + 当前帧和上一帧需要足够的加速度。

初始位姿的设置：

• 纯双目视觉SLAM，设置初始旋转为单位阵，初始平移为0；
• 双目+IMU模式，设置初始旋转和平移为相机在IMU的body坐标系下的位置，设置初始速度为0

地图点描述子的设置
找到观测到所有该地图点的关键帧–>在每一个关键帧找到pNewMP对应的特征点的描述子->计算所有描述子两两之间的距离->将每个描述子和其他描述子的距离进行排序并寻找中值 ->比较所有描述子对应的中值–>选择最小中值对应的描述子作为pNewMP的描述子

13.单目初始化的步骤

单目模式的初始化主要包括并行地计算基础矩阵和单应性矩阵，根据投影误差选取其中一个模型，恢复出最开始两帧之间的相对姿态以及点云，计算初始两帧的匹配、相对运动、初始MapPoints,在单目初始化时，IMU并未起到作用，只是更新了预积分器。

• Step 1：（未创建）得到用于初始化的第一帧，初始化需要两帧
• Step 2：（已创建）如果当前帧特征点数大于100，则得到用于单目初始化的第二帧
• Step 3：在mInitialFrame与mCurrentFrame中找匹配的特征点对(搜索半径100，描述子计算，旋转一致性检验)
• Step 4：如果初始化的两帧之间的匹配点太少(小于100)，重新初始化
• Step 5：通过H模型或F模型进行单目初始化(基于RANSAC的8点法，基于卡方检验的模型选择)，得到两帧间相对运动(r，t)、初始MapPoints
• Step 6：计算三角化恢复的3D点相对两个相机光心视差和数量是否满足要求
• Step 7：将三角化得到的3D点包装成MapPoints

14.寻找两帧之间特征点匹配的tricks

• 即根据初始帧特征点的坐标，在匹配帧相应坐标附近搜索初始帧中该特征点的匹配点
• 为了获得比较好的匹配，需要满足最佳匹配的描述子距离要比较明显地小于次佳匹配的描述子距离
• 旋转一致性检验：对所有的匹配点对之间的角度差进行统计并构建一个基于角度差的分布直方图，排除角度差和大多数相差较大的匹配对

15.特征点坐标归一化处理

因为噪声、数值的四舍五入、误匹配等情况的存在，直接基于像素点计算得到的基础矩阵F会存在较大的偏差，因此在求解基础矩阵前需要对特征点像素坐标进行归一化处理，最终得到一组去均值去中心化的归一化坐标，和一个由特征点像素坐标到归一化坐标的变换矩阵T。

这里的归一化处理只是对特征点像素进行去均值和去中心化，并没有使用内参进行反投影，因此计算出来的依然是基础矩阵。

16.基础矩阵和单应矩阵的计算方法

基于RANSAC迭代的8点法，迭代200次，每次迭代选择8个点计算基础矩阵(单应矩阵)，然后根据基础矩阵(单应矩阵)计算投影误差，基于投影误差进行卡方检验并计算得分，如果得分满足阈值停止迭代。

17.本质矩阵计算

基础矩阵和本质矩阵都是从对极约束公式中得到的，考虑到两个相互匹配的归一化坐标或者平面像素坐标，根据对极约束具备以下关系：

从对极约束关系中可以得到基础矩阵和本质矩阵的表达形式，基础矩阵对应像素坐标的几何关系，本质矩阵对应归一化平面坐标的几何关系。

本质矩阵的自由度问题
因为对极约束是等式为零的约束,所以对E乘以任意非零常数后,对极约束依然满足，因此本质矩阵具有尺度等价性。

另一方面,由于平移和旋转各有三个自由度,故t^R共有六个自由度。但由于尺度等价性,故E实际上有五个自由度,因此最少可以通过五个点对求解本质矩阵。

但事实上E的内在性质是一种非线性性质,在求解线性方程时会带来麻烦,因此,也可以只考虑它的尺度等价性,使用八对点来估计E，这就是经典的八点法。

1个3×3矩阵是本质矩阵的充要条件是它的奇异值有两个相等且第三个为0，根据这一性质反推知道本质矩阵的奇异值分解有两种形式，可求得两个对应的旋转矩阵和2个平移向量，进行排列组合则有4组对应解。

求解本质矩阵

在得到线性方程组之后可以基于SVD分解，得到对应的t和R(四组解)，然后将任一点带入计算深度，根据该点在两个相机视野下的深度信息判断唯一解。

18.基础矩阵的求解

根据对极约束等式，也可以使用像素坐标根据基础矩阵进行求解，基础矩阵也具有尺度等价性，可以使用8点法进行求解，得到类似的方程，然后通过SVD分解可以得到相应的平移向量和旋转矩阵。

代码中为什么对F矩阵求解时进行两次SVD分解
因为基础矩阵具备尺度等价性，其秩为2,第一次SVD分解得到的是一个普通的变换矩阵，因此需要将第3个奇异值设置为0然后进一步对基础矩阵进行约束。

19.单应矩阵的求解

单应矩阵描述了两个平面之间的映射关系，当特征点共面,或者相机发生纯旋转的时候,基础矩阵的自由度下降,这就出现了所谓的退化(degenerate)。现实中的数据总包含一些噪声,这时候如果我们继续使用八点法求解基础矩阵,基础矩阵多余出来的自由度将会主要由噪
声决定。

对于平面上某点满足一下方程：

带入到相互匹配的一对特征点关系中可以得到：

然后类似与本质矩阵和基础矩阵的求解，同样可以得到一个线性方程,理论上H矩阵的自由度为8只需要4对点即可进行求解，ORB-SLAM中为了增加求解的准确性同样采用的8点法，最后对H矩阵进行SVD分解可以得到旋转矩阵R和平移向量t。

20.单目初始化成功的判断条件

在得到本质矩阵或者单应矩阵后，会进行SVD分解得到平移向量和旋转矩阵，根据R和t会恢复特征点对应的3D点。

因为可以从基础矩阵中恢复出4组r和t，只有存在一组解恢复出的3D点大于90%的匹配点数量时才认为三角化成功。

21.IMU需要标定的参数

（1）与相机(左目相机)之间的外参
（2）IMU线加速度和角速度的白噪声，即测量噪声，假设服从高斯分布
（3）IMU线加速度和角速度的随机游走，表示偏置随时间变化的导数

22.IMU预积分时由于时间戳无法完全和图像帧时间戳对齐的补偿问题

对应vins中的时延问题的处理。

在ORB-SLAM3算法中还是以图像帧为主，因此对IMU数据采用类似插值的方法，对于和图像帧时间戳接近IMU数据采用补偿的方式，目的在于只计算图像帧时间戳之内的预积分量。

在VINS系列中，采用基于滑窗的VIO算法，主要以IMU数据的估计为主，因此对图像帧的运动采用插值的方法，这里采用了和ORB-SLAM相似的恒速模型，认为相邻的帧间运动相同，然后根据图像帧时间戳和IMU时间戳之间的时间间隔，算出这段时间内特征点运动的大小,和在IMU时间戳对应时刻特征点的位置。

23.基于恒速模型的帧间跟踪

首先是IMU模式，如果预积分存在并且最近没有进行过重定位，认为预积分结果可信，直接返回位姿。

其他情况就是纯视觉模式的恒速模型跟踪，ORB-SLAM认为相邻图像帧之间的运动相等，即认为k-2时刻到k-1时刻的相机运动和k-1时刻到k时刻的相机运动相等，这样就可以根据k-2时刻到k-1时刻的相机运动以及上一帧的位姿得到当前帧的初始位姿。

然后就是将上一帧的3D点在当前帧进行投影匹配，并根据旋转一致性筛除外点，最后基于重投影误差函数优化当前帧相机位姿，在优化后如果内点数量满足要求则认为跟踪成功。

24.跟踪参考帧

无论是跟踪恒速运动模型还是跟踪参考关键帧，本质上都是基于帧间匹配跟踪。

跟踪恒速模型是当前帧和上一帧之间的匹配，使用基于恒速模型计算得到的位姿作为优化的初始位姿，基于网格和搜索半径寻找匹配关系。

跟踪参考关键帧是当前帧和参考关键帧之间的匹配，虽然在时序上两个图像帧不连续，但是在空间上参考关键帧和当前帧的视野具有极高的重合度，因此可以作为投影匹配的参与对象，和跟踪恒速运动模型不同的是跟踪参考关键帧使用词袋向量搜索匹配关系，并使用上一帧lastframe的位姿作为当前帧位姿优化的初始位姿。

在得到两帧之间的匹配之后，跟踪恒速模型和跟踪参考关键帧的步骤基本就类似了，首先基于非线性优化计算当前帧的位姿，并过滤匹配误差较大的外点，得到最终的匹配数量，根据匹配数量是否满足要求判断是否跟踪成功。

25.单词与词典树

参考链接：https://blog.csdn.net/u011341856/article/details/109405181

视觉词典的本质是一个通过聚类得到的KD树，通过离线训练，对各种类型的特征点对应的描述子进行聚类，划分成不同的簇，然后以簇的聚类中心作为子节点，再对子节点中的描述子进行聚类划分子节点的子节点，以此循环，直到满足条件。

1.词典的创建

如下图所示，假设在训练时一共使用了1000个描述子，首先我们将这1000个描述子进行聚类划分成3个簇，每个簇分别有200,300,500个元素，其中每个簇对应的聚类中心作为kd树第一层的字节点。

然后再对每个字节点对应的簇继续聚类，各自再划分为3个簇，然后形成第二层的9个子节点。

如果不再继续聚类的话，最终我们得到一个分支为3深度为2的kd树。而最下面一层，每个簇的聚类中心被曾为单词，即该图的kd树有9个单词。

当我们保存整个训练好的词汇树时，只要把树的结构和聚类中心都保存下来就行了，其中那些用来训练的描述子都可以扔掉了。

整个过程总结如下：

• step1.在根节点将样本聚为k类
• step2.对第一层的每个节点，把属于该节点的样本再聚为k类
• step3.重复step2，最后得到叶子层，每个叶子对应一个word，最终得到的树就是vocabulary tree

2.基于词袋向量搜索的特征匹配

假设对于当前帧图像提取了1000个特征点，对应的有1000个描述子，在ComputeBoW()函数中我们已经将整幅图像中的描述子转化为了词袋向量，同时记录了每个描述子对应的叶节点id。至于怎么计算的每个描述子对应的节点信息，就是和每一层节点(聚类中心描述子)计算描述子距离，和哪个节点的描述子距离最近该描述子就属于哪个节点。

在进行特征匹配时，为了加速匹配只计算属于同一节点的描述子以寻找匹配关系，这样就将1000对1000的暴力匹配转换为某个节点的小范围暴力匹配，节省计算时间。

每完成一个节点中的特征匹配，就要对这些匹配进行验证：

• 相互匹配的两个特征点的描述子距离要小于设定的阈值
• 最佳匹配的描述子距离要比较明显的小于次佳匹配的描述子距离(在SearchByBoW中最佳距离要小于0.7倍的次佳距离)

最后完成所有的特征匹配后需要对匹配的特征旋转偏差进行旋转一致性验证，然后记录最终的匹配关系，用于后续的非线性优化。

3.基于词袋向量的回环检测和重定位

参考链接：https://zhuanlan.zhihu.com/p/354616831

回环检测和重定位在本质上都是场景识别，在这两个模块中都使用了词袋向量进行了加速匹配。上面我们介绍了怎么为每一个特征点对应的描述子寻找对应的叶节点(也就是单词)，假如我们在一张图像中提取了1000个特征点，对应1000个描述子，通过计算我们将这1000个描述子分别找到了对应的叶节点或者说单词，这样最终一帧图像可以用若干个单词进行表示。

随着SLAM过程的进行会不断的创建关键帧，同时会将关键帧转化为词袋向量，在进行回环检测和重定位时会遍历所有关键帧，其中和当前帧公共单词数最多的关键帧就是当前帧的最佳候选关键帧。

所谓倒排索引即对于某个单词可以通过搜索关键帧数据库查找拥有该单词的所有关键帧，这样对当前关键帧所有的单词都进行逆序查找，会得到当前关键帧与相似关键帧之间的公共单词数，进而确定候选关键帧。

26.简述重定位过程

重定位是ORB-SLAM系列保持跟踪稳定性的保障，从ORB-SLAM沿用至ORB-SLAM3。主要作用是在跟踪失败时，通过词袋向量搜索在关键帧数据库中寻找和当前帧相似的关键帧作为匹配帧，建立数据关联并计算当前帧的位姿，恢复相机的运动。

27.RENCENT_LOST状态的判定条件

当上一帧在跟踪运动模型环节TrackWithMotionModel()或者跟踪参考关键帧环节TrackReferenceKeyFrame跟踪失败时，会对当前帧的跟踪状态有一个判断。

• 条件1：当前地图中关键帧数目较多（大于10）
• 条件2（隐藏条件）：当前帧距离上次重定位帧超过1s（说明还比较争气，值的救）
• 条件3：不是IMU模式

同时满足条件1，2 || 1,3 则将状态标记为RECENTLY_LOST;如果是纯视觉模式，则在下一帧会进入重定位函数；如果是VI模式，则在下一帧会通过预积分更新位姿。

28.重定位过程

• 将当前帧的描述子转换为词袋向量
• 在当前子地图中寻找与当前帧相似的候选关键帧组，即拥有较多公共单词数的关键帧
• 遍历所有的候选关键帧，通过BoW进行特征点的快速匹配，用匹配结果初始化PnP Solver
• 使用基于RANSAC的MLPNP算法求解位姿，然后筛选外点，当内点数量大于50个是认为重定位成功

29.词袋向量的正向索引和逆向索引

(1)正向索引：
主要用于加速特征匹配，在进行特征匹配时可以先判断特征点对应描述子和某一层的哪个节点的聚类中心最接近，然后去该节点中对应的特征点寻找匹配点。

(2)逆向索引：
逆向索引就是对于某个单词查找包含这个单词的关键帧。遍历当前帧的单词集合，对于每个单词，它里面落入了许多历史关键帧，对这些帧计数+1，表示与当前帧共享一个单词，统计完当前帧的所有单词之后，取共享数量最多的那一帧，就是与当前帧最接近的一帧了。

30.重定位寻找候选关键帧的方法

• 先通过BOW词袋向量搜索找出和当前帧具有较多公共单词数的关键帧
• 对于每个候选关键帧，选择与其共视程度最高，权重最高的前10个关键帧划分为一组
• 遍历每一个组的关键帧，统计每个组得分最高的关键帧，以及总得分最高的组对应的最高分数bestAccScore
• 根据最高分数bestAccScore再从每个组得分最高的关键帧里挑选大于阈值的关键帧，然后去掉不属于当前活跃子地图的关键帧，其余的作为最后的候选关键帧返回

31.相似性得分计算方法

基于L1范数计算两个词袋向量的距离，实质上就是两个向量对应元素之和减去对应元素之差。

计算相似性得分时并不是根据单词的有无进行0,1的比较,而是根据单词出现的频率计算一个TF-IDF权重,所以词袋向量中每个元素都表征了每个单词在图像中出现的频率.

单词权重和相似性得分的计算方式如下:

32.简述局部建图线程

在ORB-SLAM中，局部建图线程的作用之一就是为了跟踪局部地图而服务的，该线程和跟踪线程同时启动并列运行。局部建图线程时刻维护一个共视地图，这个局部地图包括和当前帧存在共视关系的关键帧，共视关键帧的共视关键帧、这些关键帧对应的地图点。搭载相机的智能体每经过一帧，局部建图线程均会根据环境的情况实时地更新局部地图。

主要过程：

• 更新局部地图中的关键帧和地图点
• 将局部地图中的点投影到当前帧寻找匹配关系
• 基于非线性优化对当前帧位姿进行优化（如果IMU成功初始化则需要同时优化当前帧对应的位姿，速度和IMU偏置，其他情况仅对当前帧位姿进行优化）
• 根据优化后剩余内点的数量确定是否跟踪成功

33.局部地图中关键帧的类型

• 类型1：能观测到当前帧地图点的关键帧作为局部关键帧(一级共视关键帧）
• 类型2:一级共视关键帧的共视关键帧(二级共视关键帧)
• 类型3:将一级共视关键帧的子关键帧
• 类型4:将一级共视关键帧的父关键帧
• 类型5:当前帧的往前数20个连续的关键帧(仅IMU模式)

34.关键帧的创建条件

主要基于两个原则，在间隔时间较长或者匹配较少时插入关键帧，IMU模式下间隔时间稍长一些，IMU预积分需要有足够的运动。

1. IMU未初始化时，距上一次关键帧插入间隔大于0.25秒时选择插入关键帧
2. 纯定位模式不插入关键帧
3. 局部建图线程停止接收关键帧时不插入(一般是被闭环检测线程占用)
4. 距离上次重定位较近且当前关键帧数目还算较多不插入,阈值为图像帧率FPS
5. 跟踪局部地图之后匹配较少,且很长时间没有插入关键帧时选择插入(1s)
6. ORB_SLAM3考虑了IMU的不稳定性,如果当前帧和上一关键帧之间时间超过0.5秒选择插入
7. ORB_SLAM3单目+IMU模式匹配点低于75或者状态为RECENTLY_LOST插入

35.关键帧的删除条件

要删除的是局部地图中当前关键帧的共视关键帧中的冗余部分，因为地图点是伴随局部地图中的关键帧存在的，会随着关键帧的删除消失在局部地图中。

如果局部地图中，当前关键帧的共视关键帧有90%的点能够被其他至少3个关键帧看到，说明这个关键帧的信息冗余，删之。

36.VI模式下的三步初始化

ORB-SLAM3的系统初始化分成了三个主要的模块：纯视觉初始化、纯IMU初始化、视觉和IMU联合优化。

纯视觉初始化和之前一样就是单目或者双目初始化，在Tracking线程中进行；

纯IMU初始化和视觉与IMU联合优化目的是为了通过视觉创建的关键帧以及关键帧之间的积分和位姿，获取精确的IMU不确定度即偏置信息，减小IMU传感器数据的不稳定性。

纯IMU的初始化要进行三个阶段，目的是为了不断的优化IMU传感器的不确定性即IMU偏置、scale等参数，并联合视觉定位信息进行联合优化，进一步优化智能体的位姿和地图点信息。

第一阶段的初始化，只对IMU的偏置bias信息、scale以及相关残差进行优化，不涉及与视觉和IMU的联合优化。

为了获得足够的运动，IMU的第二次初始化需要在第一次初始化5s之后再进行，第三次需要在第二次成功后15s进行。第二次初始化，会对IMU数据做一次BA优化

第二次初始化和第三次初始化在对IMU参数进行优化后，需要联合视觉信息进行联合BA优化

37.回环检测概述

ORB-SLAM3中的回环检测线程主要分为4个模块：检测共视区域、检测是否存在回环、计算相似变换、回环矫正

共视区域检测主要是看关键帧和之前已经创建的关键帧之间是否存在公共单词，如果存在再进一步分析检测到的共视关键帧是其他子弟图的还是当前子地图的

• 如果检测到的共视关键帧是其他子地图的，则将两个子地图进行融合
• 如果检测到的共视关键帧是当前子地图的，则有可能存在回环关系
• 如果存在回环关系则计算回环关键帧之间的相似变换，获取漂移的程度
• 如果相似变化计算成功，则根据漂移程度纠正回环处的累计误差

38.共视区域检测原理

和重定位候选关键帧采用同样的方法，在以往的关键帧数据库中挑选具有公共单词数的关键帧，再计算相似性得分(1范数)，挑选得分高的3个候选关键帧返回。

挑选的时候，如果关键帧来自其他子地图则是子地图融合候选，如果是来自同一个子地图就是回环候选

39.检测是否存在回环或者融合的可能性

大致原理：首先通过回环候选关键帧(这是之前存在关键帧数据库中的)与当前帧进行投影匹配计算出一个sim相似变换矩阵，计算方法就是RANSAC和非线性优化求解

基于这个初步计算得到的sim矩阵进行几何信息验证：这时选择当前帧的5个共视程度较高的共视关键帧(这是当前位置处的关键帧)，然后使用投影匹配计算匹配的内点数量，如果数量大于阈值则通过验证，如果5帧里面有3帧通过了验证，则认为存在回环或者存在子地图融合区域。

40.回环矫正过程

如果检测到的是回环的情况，在IMU模式下还会对回环处的raw,pitch和yall角度进行验证，验证通过则进行回环矫正。

在之前回环检测时已经初步获得了当前帧和回环关键帧之间的相似变换矩阵，则可以通过传播思想先初步将当前帧的共视关键帧，以及共视关键帧的共视关键帧均相应的乘以一个相似变换

根据调整后的位姿调整相应地图点的位置

然后先针对回环处，对当前关键帧的本质图进行优化，对所有的位姿和地图点进行全局BA优化，最终得到全局一致性的位姿和地图

41.共视图、本质图、扩展树

Covisibility Graph
对于某个区域的3D地图点，投影在图像上就是特征点，就是该图像观测到了这个地图点。如果这个3D地图点被两个图像观测到，就称这两张图像存在共视关系。
在共视图Covisibility Graph中，每个节点代表一个关键帧，共视关键帧之间通过权重θ进行连接，权重θ即两个关键帧共视地图点的数量。在ORB-SLAM中只有当共视地图点大于15时才会进行连接(15应该是作者实验出的经验值)。

Essential Graph
本质图Essential Graph主要为回环检测服务，Essential Graph包含了一个生成树，一个高covisibility（θmin=100）的covisibility graph边缘子集，以及闭环回路的边缘，这样的组合共同构建了一个强大的相机网络。

生成树Spanning Tree：
每个关键帧都会存在很多和共视关键帧，在进行连接时将某个关键帧指向和其共视关系最好、权重最大的关键帧，根据这种规则从初始关键帧开始创建，就可以的到一个生成树。
如图，5和3、4存在共视关系，但是5和4之间共视关系最好，所以将箭头由5指向3.

42.跟踪运动模型、跟踪参考关键帧、跟踪局部地图、重定位中使用的匹配方式

跟踪运动模型：使用上一帧的3D点进行投影匹配，基于网格注册信息和搜索半径加速匹配
跟踪参考关键帧：基于词袋向量加速匹配特征点的搜索，只对属于同一节点的描述子进行计算
重定位：基于词袋向量加速匹配特征点的搜索，使用基于RANSAC的pnp迭代求解
跟踪局部地图：将局部地图中所有的3D点投影到当前帧，基于网格注册信息和搜索半径加速匹配

43.PNP,P3P,EPNP和MLPNP之间的比较

pnp问题是已知一组相匹配的3D点和2D点，求解他们之间的投影关系的问题。

P3P,EPNP和MLPNP都是求解PNP问题的具体算法。

1.P3P
P3P利用3个点的几何关系，通过三对点已知的世界坐标系下的3D坐标以及相应点在相机归一化平面上投影的2D坐标，可以通过构建方程得到三个点在相机坐标系下的3D坐标，之后将问题转化为3D-3D的ICP问题。

2.EPNP
核心思想是将3D点转换成由4个控制点线性组合的方式参与求解,用于重定位，与其他方法相比，EPnP方法的复杂度为O(n)，通过将n个点对n个点之间关系的求解转换为4个控制点对4个控制点之间的关系求解，对于点对数量较多的PnP问题，非常高效。

EPnP算法利用已知的n个3D点的世界坐标，通过PCA选择4个控制点(世界坐标已知)，建立新的局部坐标系，从而将3D点的世界坐标用新的控制点(世界坐标)表示出来。然后，利用相机投影模型和2D点像素坐标，建立关于控制点在相机坐标系下坐标的线性方程组，求解出4个控制点在相机坐标系下的坐标，进而求解出n个3D点在相机坐标系下的坐标。到了这里，我们已知n个3D点在世界坐标系下和相机坐标系下的坐标，问题转换为3D-3D问题，利用ICP求解即可。

参考链接：https://blog.csdn.net/jessecw79/article/details/82945918

**EPNP控制点的选择：**第一个选择世界坐标系下3D点的质心；通过对3D点去质心化可以得到矩阵A^TA,根据该矩阵的特征值和特征向量可以求出剩余三个控制点的坐标。

在得到四个控制点之后，对于世界坐标系下的这组3D点都可以用这组控制点的线性化组合进行表示，同样的对于这组3D点变换到相机坐标系后也可以同样找到一组控制点进行相应表示。

根据相机的位姿可以将世界坐标系下3D点和相机坐标系下3D点之间的变换关系，转换成他们相应控制点的变换关系。

现在的问题就变成，已知3D点在世界坐标系下的控制点坐标，在图像平面的投影坐标，求解在相机坐标系下对应的控制点和相机位姿。

**相机坐标系下控制点的求解：**根据相机内参可以将2D的像素点反投影成相机坐标系下的3D点，根据公式以及n个2D像素点可以得到一个线性方程M^TMX=0,对矩阵M^TM求特征值和特征向量可以得到相机坐标系下3D点对应的控制点。根据位姿可以联立世界坐标系下控制点和相机坐标系下控制点的方程，并通过SVD分解可以得到旋转矩阵R和平移向量t.

3.MLPNP
MLPNP的核心思想是对于每一个3D空间点，连接空间点到相机光心可以得到一个法向量v，以及在切平面对应的一组正交基[r,s],然后将2D-3D点的投影匹配转换为使用正交基之间进行计算。