第5周课件-全网最详细的ORB-SLAM2精讲

本文链接：https://blog.csdn.net/hltt3838/article/details/113831640

本周课程重点：

1. 掌握恒速模型跟踪、重定位、局部建图跟踪原理及代码（非常重要）。

2. 理解关键帧的概念和应用。

1 恒速模型跟踪TrackWithMotionModel

对应函数 Tracking::TrackWithMotionModel() ，示意图如下

应用场景

大部分时间都用这个跟踪，只利用到了上一帧的信息。

1. 用恒速模型先估计一个初始位姿

2. 用该位姿进行投影匹配 SearchByProjection ，候选点来自 GetFeaturesInArea ，未使用 BoW

3. BA 优化（仅优化位姿），提供比较粗糙的位姿

思想

假设短时间内（相邻帧）物体处于匀速运动状态，可以用上一帧的位姿和速度来估计当前帧的位姿。移动模式跟踪前后两帧得到变换矩阵。上一帧的地图3d 点反投影到当前帧图像像素坐标上，在不同尺度下不同的搜索半径内，做描述子匹配搜索可以加快匹配。在投影点附近根据描述子距离进行匹配（需要>20 对匹配，否则匀速模型跟踪失败 , 运动变化太大时会出现这种情况），然后以运动模型预测的位姿为初值，优化当前位姿，优化完成后再剔除外点，若剩余的匹配依然>=10 对，则跟踪成功，否则跟踪失败，需要 Relocalization

具体流程

1 ：创建 ORB 特征点匹配器，最小距离 < 0.9* 次小距离，匹配成功

2 ：更新上一帧的位姿和地图点，单目：只计算了上一帧世界坐标系位姿就退出了。Tlr*Trw = Tlw 双目或rgbd 相机：根据上一帧有效的深度值产生为上一帧生成新的临时地图点，之所以说是 “ 临时 ” 因为

这些新加的地图点不加入到 Map 中，只是为了当前帧间跟踪更稳定，用完会删除，过河拆桥啊！

3 ：使用当前的运动速度 ( 之前前后两帧位姿变换 ) 和上一帧的位姿来初始化当前帧的位姿 R,t

4 ：在当前帧和上一帧之间搜索匹配点（ matcher.SearchByProjection) 通过投影( 使用当前帧的位姿 R,t) ，对上一帧的特征点 ( 地图点 ) 进行跟踪 . 上一帧3d 点投影到当前坐标系下，在该 2d 点半径 th 范围内搜索可以匹配的匹配点遍历可以匹配的点，计算描述子距离，记录最小的匹配距离，小于阈值的，再记录匹配点特征方向差值进行方向验证，剔除方向差直方图统计中，方向差值数量少的点对，保留前三个数量多的点对。

5 ：如果找到的匹配点对如果少于 20 ，则扩大搜索半径 th=2*th, 使用 SearchByProjection() 再次进行搜索。

6 ：使用匹配点对对当前帧的位姿进行优化 G2O 图优化

7 ：如果 2d-3d 匹配效果差，被标记为外点，则当前帧 2d 点对于的 3d 点设置为空，留着以后再优化

8 ：根据内点的匹配数量，判断跟踪上一帧是否成功。

2 跟踪丢失后的重定位Relocalization

对应函数 Tracking::Relocalization()

应用场景

跟踪丢失的时候使用，很少使用。利用到了相似候选帧的信息。

1. 用 BoW 先找到与该帧相似的候选关键帧（函数 DetectRelocalizationCandidates， 可以找到很多的候选帧 ）

2. 遍历候选关键帧，用 SearchByBoW 快速匹配，

3. 匹配点足够的情况下用 EPnP 计算位姿并取出其中内点做 BA 优化（仅优化位姿），

4. 如果优化完内点较少，通过关键帧投影生成新的匹配（函数 SearchByProjection ），

5. 对匹配结果再做 BA 优化（仅优化位姿）。

思想

当TrackWithMotionModel 和 TrackReferenceKeyFrame 都没有跟踪成功，位置丢失后，需要在之前的关键帧中匹配最相近的关键帧，进而求出位姿信息。使用当前帧的BoW 特征映射，在关键帧数据库中寻找相似的候选关键帧，因为这里没有好的初始位姿信息，需要使用传统的 3D-2D 匹配点的EPnP 算法来求解一个初始位姿，之后再使用 最小化重投影误差 来优化更新位姿

具体流程

1 ：计算当前帧的 BoW 向量和 Feature 向量

2 ：在关键帧数据库中找到与当前帧相似的候选关键帧组

3 ：创建 ORB 特征点匹配器，最小距离 < 0.75* 次小距离匹配成功。 ORBmatcher matcher(0.75,true);

4 ：遍历每一个候选关键帧使用 BOW 特征向量加速匹配，匹配太少的去掉，选择符合要求的候选关键帧用其地图点为其创建pnp 优化器

5 ：使用 PnPsolver 位姿变换求解器 , 更加 3d-2d 匹配点，6点直接线性变换 DLT, 后使用 QR 分解得到 R,t, 或者使用 (P3P) ， 3 点平面匹配算法求解。这里会结合 Ransac 随采样序列一致性算法，来提高求解的鲁棒性。

6 ： EPnP 算法迭代估计姿态作为当前帧的初始位姿，使用最小化重投影误差 BA 算法来优化位姿

7 ：如果优化时记录的匹配点对内点数量少于 50 ，想办法再增加匹配点数量：通过投影的方式对之前未匹配的点进行3D-2D 匹配，又给了一次重新做人的机会

8 ：如果新增的数量加上之前的匹配点数量大于 50 ，再次使用位姿优化算法进行优化

9 ：如果上面优化后的内点数量还比较少，还想挽留一下，就缩小搜索窗口重新投影匹配（比之前使用更多的地图点了），如果最后匹配次数大于50 ，就认为是可以勉强扶起来的阿斗，再给 BA 位优化一次。否

则，放弃了（真的已经仁至义尽了！）

10 ：如果经过上面一系列的挽救操作，内点数量大于等于 50 ，则重定位成功。

检测重定位候选关键帧

对应函数 DetectRelocalizationCandidates

PnP 简介

PnP 投影示意图

参考

ORBmatcher::SearchByProjection

为什么要用 EPnP?

论文：EPnP: Accurate Non-Iterative O(n) Solution to the PnP Problem 。计算复杂度低，是 O(n) 的只需要4 个及以上的控制点，适用于平面、非平面精度比较高

坐标系变换 ：疑问：tlc = Rlw*twc+tlw; 原因？（ 看下面的公式推导 ）

地图点

地图点是三维点，来自真实世界的三维物体，有唯一的id 。不同帧里的特征点可能对应三维空间中同一个三维点，特征点是二维点，是特征提取的点，大部分二维点在三维空间中没有对应地图点

关于生成地图点，主要有2 个地方：

1 、初始化时前两帧匹配生成地图点

2 、 local mapping 里共视关键帧之间用 LocalMapping::CreateNewMapPoints() 生成地图点

3 、 Tracking::UpdateLastFrame() 和 Tracking::CreateNewKeyFrame() 中为双目和 RGB-D 生成了新的 临时地图点 ，单目不生成 倒排索引 ，// mvInvertedFile[i]表示包含了第 i 个 word id 的所有关键帧

std::vector<list<KeyFrame*> > mvInvertedFile; 直接索引表 direct index 和逆向（倒排）索引表 inverse index 。直接索引表是以图像为基础的，存储图像中的特征以及和该图像相关联的节点 node （ vocabulary tree 的某一层级）。 直接索引的优势： 通过同一个节点下特征点描述子对应关系加速几何关系验证，比如用BOW 快速匹配逆向（倒排）索引表是以 Word 为基础，存储 Word 的权重以及该 Word 出现在哪个图像里。这对于查询数据库非常方便，因为它可以很方便对比哪些图像有共同的Word

3. 局部地图跟踪 TrackLocalMap

对应函数 tracking::TrackLocalMap

简介

应用场景： 前面3 种跟踪方式得到当前帧地图点后的后处理，每次跟踪都使用。前提是必须知道当前帧的位姿和地图点（尽管不准确），利用到了当前帧的两级共视关键帧的信息，使得位姿更加准确。

具体步骤 ：

首先根据前面得到的当前帧的地图点来找能观测到当前帧的一级共视关键帧，将这些一级共视关键帧的二级关键共视帧、子关键帧、父关键帧一起作为局部关键帧; 取出上述局部关键帧中所有的地图点作为局部地图点；将局部地图点投影到当前帧，去掉不在视野内的无效的地图点，剩下的局部地图点投影到当前帧进行匹配（函数SearchByProjection ）对匹配结果再做BA 优化（仅优化位姿）， 当前帧： mCurrentFrame （当前帧是普通帧），

参考关键帧 : 与当前帧共视程度最高的关键帧作为参考关键帧， mCurrentFrame.mpReferenceKF ，在KeyFrame::UpdateConnections() 里确定关键帧的父子关系（当前帧必须是关键帧）

父关键帧： 和当前关键帧共视程度最高的关键帧

子关键帧： 是上述父关键帧的子关键帧，

mvpLocalMapPoints 示意图 就是下图中红色地图点，用来在TrackLocalMap 里跟踪当前帧

mvpLocalKeyFrames 示意图

换底公式

局部地图的构成以及工作流程

跟踪过程整体流程图

参考： https://blog.csdn.net/u014709760/article/details/89465842#21_Tracking__6

4. 关键帧

什么是关键帧：通俗来说，关键帧就是几帧普通帧里面具有代表性的一帧。

为什么需要关键帧

相近帧之间信息冗余度很高（通俗的讲就是相邻帧的相似度很高，没有必要全部记录），关键帧是取局部相近帧中最有代表性的一帧，可以 降低信息冗余度 。举例来说，摄像头放在原处不动，普通帧还是要记录的，但关键帧不会增加。关键帧选择时还会对图片质量、特征点质量等进行考察，在Bundle Fusion 、 RKD SLAM 等 RGB-D SLAM相关方案中常常用普通帧的深度投影到关键帧上进行深度图优化，一定程度上关键帧是普通 帧滤波和优化的结果 ，防止无用的或错误的信息进入优化过程而破坏定位建图的准确性。如果所有帧全部参与计算，不仅浪费了算力，对内存也是极大的考验，这一点在前端vo 中表现不明显，但在后端优化里是一个大问题，所以关键帧主要作用是面向后端优化的算力与精度的折中 ，使 得有限的计算资源能够用在刀刃上 ，保证系统的平稳运行。假如你放松 ORB_SLAM2 关键帧选择条件，大量产生的关键帧不仅耗计算资源，还会导致local mapping 计算不过来，出现误差累积 如何选择关键帧，选择关键帧主要 从关键帧自身和关键帧与其他关键帧的关系2 方面来考虑。 关键帧自身质量要好 ，例如不能是非常模糊的图像、特征点数量要充足、特征点分布要尽量均匀等等；关键帧与其他关键帧之间的关系，需要和局部地图中的其他关键帧有一定的共视关系但又不能重复度太高，以达到既存在约束，又尽量少的信息冗余的效果。

选取的指标主要有：

（ 1 ）距离上一关键帧的帧数是否足够多 （时间） 。比如我每隔固定帧数选择一个关键帧，这样编程简单但效果不好。比如运动很慢的时候，就会选择大量相似的关键帧，冗余，运动快的时候又丢失了很多重

要的帧。

（ 2 ）距离最近关键帧的距离是否足够远 （空间） / 运动，比如相邻帧根据pose 计算运动的相对大小，可以是位移也可以是旋转或者两个都考虑，运动足够大（超过一定阈值）就新建一个关键帧，这种方法比第一种好。但问题是如果对着同一个物体来回扫就会出现大量相似关键帧。

（ 3 ）跟踪局部地图质量 （共视特征点数目） 记录当前视角下跟踪的特征点数或者比例，当相机离开当前场景时（双目或比例明显降低）才会新建关键帧，避免了第2 种方法的问题。缺点是数据结构和逻辑比较复杂。