结合 高翔老师的著作《视觉SLAM十四讲：从理论到实践》，加上小白的工程经验共同完成。建议作为笔记功能反复使用。
管理局部的机器人轨迹与路标点，搭建一个完整的软件框架。
一. 搭建VO框架
- 确定程序框架；（建立文件夹）
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之类的库中会使用它。 - 确定基本数据结构；（设计好 数据单元 与 程序处理的流程）
帧：一个帧是相机采集到的图像单元。关键帧如何选择是一个很大的问题。
路标：路标点即图像中的特征点。
帧的位姿 与 路标的位置估计相当于一个局部的SLAM问题。
*编程规范：
1）尽量保证一个类有单独的头文件和源文件，避免把许多类放在同一个文件中；
2）把函数声明放在头文件和源文件，避免把许多类放在同一个文件中；
3）把函数声明放在头文件，实现放在源文件；
4）注重算法的正确实现，以及是否便于扩展；
5）可以把数据成员改成 private或 protect接口，并添加设置和获取接口；
6）过程较为复杂的算法中，应该拆分成若干子函数编写。
这里定义五个类：Frame为帧、Camera为相机模型、MapPoint为特征点/路标点、Map管理特征点，Config提供配置参数。 - Camera类；（存储相机的内参和外参，并完成相机坐标系、像素坐标系、和世界坐标系之间的坐标变换）
*编程规范：
1）每个程序头文件里都会定义 ifndef宏，防止头文件重复引用：
2）用命名空间 namespace myslam 将类定义包裹起来，防止我们不小心定义出别的库里同名函数；
3）把常用的头文件放在一个 common_include.h文件中，避免书写很长的一段头文件；
4）把智能指针定义成 Camera的指针类型，在传递参数时，只需要用 Camera::Ptr 类型即可；
5）用 Sophus::SE3来表达相机的位姿。 - Frame类；（提供数据存储和接口）
定义数据: ID、时间戳、位姿、相机、图像等；
提取方法：创建Frame、寻找给定点对应的深度、获取相机光心、判断某个点是否在视野内等。 - MapPoint类；(表示路标点)
我们将估计路标点的世界坐标；
我们会拿当前帧提取到的与地图中的路标点匹配，来估计相机的运动；
我们要存储它对应的描述子；
我们会记录一个点被观测到的次数和被匹配到的次数，作为评价它好坏程度的指标。 - Map类；（管理着所有的路标点，并负责添加新路标、删除不好的路标等工作）
VO的匹配过程只需要和 Map打交道即可。Map类中实际存储了各个关键帧和路标点，既需要随机访问，又需要随时插入和删除，因此我们使用散列（Hash）来存储它们。 - Config类。（负责参数文件的读取，并在程序任意地方都可随时提供参数的值）
我们把构造函数声明为私有，防止这个类的对象在别处建立，它只能在 setParameterFile时构造。实际构造的对象是Config的智能指针：static shared_ptr<Config>config_ 。
我们是用 OpenCV提供的 FileStorage类。它可以读取一个 YAML文件，且可以访问其中任意一个字段。
我们通过一个模版函数 get，来获得任意类型的参数值。
二. 基本的VO：特征提取与匹配
VO的任务是：根据输入的图像，计算相机运动和特征点位置。
- 两两帧的视觉里程计；（以参考帧为坐标系，把当前帧与它进行特征匹配，并估计运动关系）
待估计的运动与这两个帧的变换矩阵构成左乘关系。
流程：
1).对新来的当前帧，提取关键点和描述子;
2).如果系统未初始化，以该帧为参考帧，根据深度图计算关键点的3D位置，返回1;
3).估计参考帧与当前帧的运动;
4).判断上述估计是否成功;
5).若成功，把当前帧作为新的参考帧，回1;
6).若失败，计连续丢失帧数。当连续丢失超过一定帧数，置VO状态为丢失，算法结束。若未超过，返回1。
VisualOdometry类给出了上述算法的实现。
1).VO本身有若干种状态：初始化、正常、丢失等;
2).把一些中间变量定义在类中，方便此类中的函数互相调用参数。
3).特征提取和匹配当中的参数，从参数文件中读取。
4).addFrame函数是外部调用的接口。使用VO时，将图像数据装入 Frame类后，调用 addFrame估计其位姿。该函数根据VO所处的状态实现不同的操作。 - 讨论；
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 仅供各位研究员试读，请购买纸质书籍。