[SLAM]（9）：设计前端知识点纲要（搭建VO框架、VO特征提取与匹配、优化PnP结果、局部地图）

结合 高翔老师的著作《视觉SLAM十四讲：从理论到实践》，加上小白的工程经验共同完成。建议作为笔记功能反复使用。

---

管理局部的机器人轨迹与路标点，搭建一个完整的软件框架。

一. 搭建VO框架

1. 确定程序框架；（建立文件夹）
   1）bin ——存放可执行的二进制；
   2）include/myslam ——存放slam模块的头文件，主要是.h。（引用：include“myslam/xxx.h”）;
   3）src ——存放源代码文件，主要是 .cpp;
   4）test ——存放测试用的文件，也是 .cpp；
   5）lib ——存放编译好的库文件；
   6）config ——存放配置文件；
   7）cmake_modules ——第三方库的 cmake文件，在使用 g2o之类的库中会使用它。
2. 确定基本数据结构；（设计好 数据单元 与 程序处理的流程）
   帧：一个帧是相机采集到的图像单元。关键帧如何选择是一个很大的问题。
   路标：路标点即图像中的特征点。
   帧的位姿 与 路标的位置估计相当于一个局部的SLAM问题。
   
   *编程规范：
   1）尽量保证一个类有单独的头文件和源文件，避免把许多类放在同一个文件中；
   2）把函数声明放在头文件和源文件，避免把许多类放在同一个文件中；
   3）把函数声明放在头文件，实现放在源文件；
   4）注重算法的正确实现，以及是否便于扩展；
   5）可以把数据成员改成 private或 protect接口，并添加设置和获取接口；
   6）过程较为复杂的算法中，应该拆分成若干子函数编写。
   
   这里定义五个类：Frame为帧、Camera为相机模型、MapPoint为特征点/路标点、Map管理特征点，Config提供配置参数。
3. Camera类；（存储相机的内参和外参，并完成相机坐标系、像素坐标系、和世界坐标系之间的坐标变换）
   
   *编程规范：
   1）每个程序头文件里都会定义 ifndef宏，防止头文件重复引用：
   2）用命名空间 namespace myslam 将类定义包裹起来，防止我们不小心定义出别的库里同名函数；
   3）把常用的头文件放在一个 common_include.h文件中，避免书写很长的一段头文件；
   4）把智能指针定义成 Camera的指针类型，在传递参数时，只需要用 Camera::Ptr 类型即可；
   5）用 Sophus::SE3来表达相机的位姿。
4. Frame类；（提供数据存储和接口）
   定义数据: ID、时间戳、位姿、相机、图像等；
   提取方法：创建Frame、寻找给定点对应的深度、获取相机光心、判断某个点是否在视野内等。
5. MapPoint类；(表示路标点)
   我们将估计路标点的世界坐标；
   我们会拿当前帧提取到的与地图中的路标点匹配，来估计相机的运动；
   我们要存储它对应的描述子；
   我们会记录一个点被观测到的次数和被匹配到的次数，作为评价它好坏程度的指标。
6. Map类；（管理着所有的路标点，并负责添加新路标、删除不好的路标等工作）
   VO的匹配过程只需要和 Map打交道即可。Map类中实际存储了各个关键帧和路标点，既需要随机访问，又需要随时插入和删除，因此我们使用散列（Hash）来存储它们。
7. Config类。（负责参数文件的读取，并在程序任意地方都可随时提供参数的值）
   我们把构造函数声明为私有，防止这个类的对象在别处建立，它只能在 setParameterFile时构造。实际构造的对象是Config的智能指针：static shared_ptrconfig_ 。
   我们是用 OpenCV提供的 FileStorage类。它可以读取一个 YAML文件，且可以访问其中任意一个字段。
   我们通过一个模版函数 get，来获得任意类型的参数值。

二. 基本的VO：特征提取与匹配

VO的任务是：根据输入的图像，计算相机运动和特征点位置。

1. 两两帧的视觉里程计；（以参考帧为坐标系，把当前帧与它进行特征匹配，并估计运动关系）
   待估计的运动与这两个帧的变换矩阵构成左乘关系。
   流程：
            1).对新来的当前帧，提取关键点和描述子;
            2).如果系统未初始化，以该帧为参考帧，根据深度图计算关键点的3D位置，返回1;
            3).估计参考帧与当前帧的运动;
            4).判断上述估计是否成功;
            5).若成功，把当前帧作为新的参考帧，回1;
            6).若失败，计连续丢失帧数。当连续丢失超过一定帧数，置VO状态为丢失，算法结束。若未超过，返回1。
   VisualOdometry类给出了上述算法的实现。
            1).VO本身有若干种状态：初始化、正常、丢失等;
            2).把一些中间变量定义在类中，方便此类中的函数互相调用参数。
            3).特征提取和匹配当中的参数，从参数文件中读取。
            4).addFrame函数是外部调用的接口。使用VO时，将图像数据装入 Frame类后，调用 addFrame估计其位姿。该函数根据VO所处的状态实现不同的操作。
2. 讨论；
   1). 在 3D-2D点存在噪声的情形下，我们要用 RANSAC的解作为初值，再用非线性优化求一个最优值。
   2).参考帧位姿估计不准时，还会出现明显的漂移。我们要关心如何把当前帧与地图进行匹配，以及如何优化地图点的问题。
   3).如何提高特征点提取、匹配算法的速度，将是特征点方法的一个重要主题。

三. 改进：优化 PnP的结果

RANSAC PnP加上迭代优化的方式估计相机位姿。在之前的 PoseEstimationPnP函数中，修改成以 RANSAC PnP结果为初值，在调用 g2o进行优化的形式。

四. 改进：局部地图

将 VO匹配到的特征点放到地图中，并将当前帧与地图点进行匹配。

流程：

1）在提取第一帧的特征点之后，将第一帧的所有特征点全部放入地图中；

2）后续的帧中，使用OptimizeMap 函数对地图进行优化。包括删除不在视野内的点，在匹配数量减少时添加新点等。（使用三角化来更新特征点的地图坐标，考虑更好地管理地图规模的策略）

3）特征匹配代码。首先，我们从地图中拿出一些候选点（出现在视野之内的点），然后将它们与当前帧的特征描述子进行匹配。

关键帧：

常见的做法是，每当相机运动经过一定间隔，就取一个新的关键帧并保存起来。后端优化的主要对象就是关键帧。

五. 小结

局部地图有两个缺点：容易丢失和轨迹漂移。

如果只关心短时间内的运动，或者 VO的精度已经满足应用需求，那么有时候你可能需要的仅仅就是一个视觉里程计，而不用完全的SLAM。

 

---

《视觉SLAM十四讲：从理论到实践》 PDF资源

下载链接：https://download.csdn.net/user/robot_starscream/uploads  仅供各位研究员试读，请购买纸质书籍。