第13讲 实践：设计SLAM系统
设计一个视觉里程计，理解SLAM软件框架如何搭建，理解视觉里程计设计容易出现的问题以及解决方法。
目录
1、工程目标
2、工程框架
3、实现
附录
1、工程目标
- 实现一个精简版的双目视觉里程计。由一个光流追踪的前端和一个局部BA的后端组成。
2、工程框架
- 视觉里程计系统最基本的结构：图像、特征和图标。
- 最基本单元是图像，在双目视觉里，是一对图像，可以称为一帧。
- 对帧提取特征，这些特征很多是2D点、
- 在图像间寻找特征的关联。若多次看到某个特征，可以用三角化计算它的3D位置，即路标。
- 两个重要模块：
- 前端：前端负责提取图像中的特征，与上一帧进行光流追踪。通过光流结果计算该帧的定位。必要时，应补充新的特征点做三角化。前端处理的结果作为后端优化的初始值。
- 后端：拿到处理后的关键帧和路标点，进行优化。
- 流程 ：前后端之间房一个地图模块来处理它们之间的数据流动。前端提取了关键帧后，往地图中添加新数据；后端检测到地图更新，运行一次优化，然后把地图中旧的关键帧和地图点去掉，保持优化规模。
3、实现
- 基本数据结构：
- Frame：id、是否为关键帧、位姿、左右图像、左右图像中的特征点。其中Pose会被前后端同时设置或访问，在Set,Get函数内加锁。
- Feature：自身2D位置、是否为异常点、所在frame、关联地图点
- MapPoint：3D位置、observations记录自己被哪些feature观察。
- Map：以散列形式记录所有关键帧和对应路标点，同时维护一个被激活（窗口）的关键帧和地图点。
- 前端
- 前端目标：根据双目图像确定该帧的位姿
- 处理逻辑：
- 前端有初始化、正常追踪、追踪丢失三种状态
- 在初始化状态，根据左右双目之间的光流匹配，寻找可以三角化的地图点，成功时建立初始地图。
- 追踪阶段中，前端计算上一帧的特征点到当前帧的光流，根据光流结果计算图像位姿。该阶段只使用左目图像。
- 若追踪到的点较少，就判定当前帧为关键帧。对于关键帧：
- 提取新的特征点
- 找到这些点在右图的对应点，用三角化建立新的路标点
- 将新的关键帧和路标点加入地图，并触发一次后端优化。
- 如果追踪丢失，重置前端系统，重新初始化。
- 后端
- 后端在启动之后，等到mao_update的的条件变量。当地图更新被触发时，从地图中拿取激活的关键帧和地图点，执行优化。
附录
- weak_ptr 提供了一个非常重要的成员函数 lock()，该函数尝试从当前的 weak_ptr 创建一个 shared_ptr 实例。如果当前的 weak_ptr 所指向的对象仍然存在（即，至少还有一个 shared_ptr 在管理该对象），lock() 函数会返回一个指向该对象的 shared_ptr 实例，并且会增加该对象的引用计数。如果 weak_ptr 所指向的对象已经被销毁（即，没有任何 shared_ptr 在管理该对象），lock() 函数会返回一个空的 shared_ptr。