SLAM代码（设计模式2）

上一个博客中，我们介绍了一些关于slam的前端的设计思想的内容，这里我们继续完成slam的前后端的设计
首先我们给出slam程序中各模块的主要组成部分。

我们先理清楚到底他们干了些什么，然后再说他们怎么实现的。其实吧，现代的CPU对多线程的代码支持的越来越好，因为木法提速，只能多个人（CPU核）干。所以在PTAM把Tracking 和 Maping 分开完之后，大家一下子就觉得这样玩真的不错，一窝蜂都转成这种玩法。

所以按照国际惯例，我们还是把前后端分开处理，前端是一个跟踪器，后端干的事还挺多的，什么建图啊，BA，闭环啊，各种，看着都累。

跟踪器（定位的过程）

跟踪顾名思义就是上一帧里边的点我们特别关心他们现在在哪里，然后呢，怎么直到他们去哪里呢？我们在上一个博客中介绍了有两种方法，一种使用局部范围的直接搜索，另外只用是使用特征描述子进行匹配。匹配正确的点用来计算他们的几何关系，根绝几何关系可以恢复出相机的运动，进而重建出世界中的三维点。这样到来新的一帧之后我们都可以提取到一些新的三维点。另外我们可以对应三维点和图像中的点进行计算相机的姿态。这就是跟踪器的主要目的。

以上内容是假设更踪器在顺利跟踪的时候要干的事，那么问题来了，如果跟踪不顺利或者这就是第一帧那怎么办？
所以我们把跟踪分为三种状态，一种是第一帧，叫做初始状态，其实在单目中第二帧也属于这个状态，因为第一帧什么也干不了，就只能提特征，第二帧来了才有匹配等等。

如果如果跟踪失败，那么这个状态我们成为lost，然后对应的措施是relocalization。 我们看到的那篇posenet就是干这个的。

在跟踪中初始化的时候，计算匹配点的集合关系使用的是的一般是单应矩阵和基础矩阵。
在跟踪时候匹配当前帧和路标点，一个图像中的点，一个三维空间中的点，因此用的是PnP，优化Pose，路标点这里是假设为静止的，只有pose被优化。

那么优化完了，我们可以得到测量数据了，但是为了完成下一次的跟踪，我们还有一些工作要做。
1. update Local KeyFrame
2. update Local Points

为什么？我们先看他到底做了哪些事就是他的目的是什么。在更新KeyFrame中增加了路标点对当前关键帧的索引。我们是通过的正匹配找到的这些路标点，然后还没有整理好这些路标点和当前关键帧的所以关系，不然后就没法使用这个帧进行匹配了，因为我们考一个参考关键帧找到他们的路标点。

初始化状态

其实初始化的内容还是比较简单，因为干的多了就容易干的久，干的久，实时性就没了，所以只有一件事就是计算前两者之间的相对位姿。一些方法只使用Homograph，有一些方法还要加上基础矩阵（极线约束），完了呢就是双视BA，有一些方法（ORB），指明了要做一个full BA（为了提高精度），为啥？ 因为开始的时候最好基础打的好一些，有信心一些不然后以后的误差太大，就玩不下去啦。

Lost state

地图（BA优化）

主要有2个待优化的内容，一是轨迹中位姿，另外一个是图中的路标点，不过很多时候路标进过几次优化之后就会不动了，那么这个时候就会使用一个pose graph来降低优化的规模，在ORB中 Covisibility Graph建模这些位姿，就是用路标点个数表明位姿节点的权重的一个图。
BA，这个东西一般是放在整个过程的最后，就是别人都折腾过（优化）一遍之后（紧着那些保证实时性的那些东西尽快的先搞出点东西来用着），把所有的东西放到一个优化器全部优化。这样做的好处就是优化过后的各部分都会精度提到，便于以后放心的使用这些东西，踢掉了不好的outliers，把网络之后的关系理顺。

由于跟踪器和建图的过程是分线程实现的，因此两者之间的同步和协调这里也要做一下，主要配合tracker，当然tracker因为肩据实时性的重要任务，那么这些工作就丢给了mapping。

mapiing其实也很忙，tracker一直再把关键帧丢过来，mapping根本就来不及处理，只好线放到一个地方存着，有一个队列deque。

功能 函数 计算关键帧特征点的BoW映射，将关键帧插入地图 ProcessNewKeyFrame() 剔除ProcessNewKeyFrame函数中引入的不合格MapPoints MapPointCulling() 相机运动过程中与相邻关键帧通过三角化恢复出一些MapPoints CreateNewMapPoints() 检查并融合当前关键帧与相邻帧（两级相邻）重复的MapPoints SearchInNeighbors() 已经处理完队列中的最后的一个关键帧，并且闭环检测没有请求停止 LocalMapping local BA Optimizer::LocalBundleAdjustment 检测并剔除当前帧相邻的关键帧中冗余的关键帧 KeyFrameCulling(); 将当前帧加入到闭环检测队列中 InsertKeyFrame

相机

• 相机的内参数
• 相机的畸变参数
• 世界空间到图像空间的变换
• 图像空间到世界空间的逆转换
• 图像点（关键特征点）的去畸变

图像帧结构

• 图像的数据
• frame ID
• frame 位姿
• 图像的特征点（关键点）提取，特征匹配
• 尺度因子（这个是特征里的对应了描述子的图像的层）
• 获取相机的光心
• 计算单应矩阵
• 计算基础矩阵
• 单应矩阵分解相机运动
• 基础矩阵分解相机运动
• 根据2帧图像三角测量点的三维位置（三维重建）
• 计算词袋

尺度的问题
特征法中，把尺度作为帧的状态，和深度关联，那么在DSO中由于残差计算的时候已经考虑了尺度。ORB中同时具备了Frame和关键帧的结构，所以关键帧中包括了三个ID号，当前帧ID，关键帧ID，上一关键帧ID

Mappoint/路标点

• 三维点，在世界坐标系下的坐标
• 描述子，这个描述子相对于其他的描述子（表征同一个三维点）的汉明距离最小。
• 可见关键帧，观测到该点的关键帧和该点点在该帧中的索引
• 该点的平均观测方向
• 参考关键帧（这个的设置是在干什么？）
• 观测距离，最小距离和最大距离根绝ORB的尺度不变形计算。

路标点和关键帧之间
- 表述该点在哪些关键帧可以看到
- 如何添加共视关系呢？维持一个std::map类型的hash，指定对于该路标点对于那些帧看到了，对应的是该帧里路标点的序号为多少。如果要删除该点，那么就要删除该hash。

路标点和描述子
一个路标点可多个描述子对应，在插入关键帧（建图的时候），对于每个路标点可以找到一个最合适的描述子，所以会出现一个过程就是获取所有的描述子，探后计算两两之间的距离，最好的描述子就是与其他距离最小的那个。描述子和每个关键帧对应。

路标点的观测方向和观测距离
观测方向的计算是所有可见关键帧的方向（相对于路标点）的平均，观测距离（深度）通过路标点减去参考关键帧的光心。

在路标点中特征法和直接法的区别
- 优化路标点

　关键帧

• 相机的位姿，从世界坐标系到相机坐标系的刚体变换
• 相机的内参数
• ORB 特征、描述子 以上这两个和图像帧结构是相同的。（特征法中的内容）
• vector<路标点>
• 删除/增加路标点
• 特征点网格
• BOW

关键帧和Covisibility Graph
Covisibility Graph的实现使用map”keyfram kf, int idx”的一种结构，前者指定了有共视关系的关键帧，后者指定了两者之间的共视关系的强弱（共视点的个数）。refer

更新具有共视关系的关键帧之间的链接的步骤

1. 首先获得该关键帧的所有MapPoint点，统计观测到这些3d点的每个关键与其它所有关键帧之间的共视程度
   对每一个找到的关键帧，建立一条边，边的权重是该关键帧与当前关键帧公共3d点的个数。
2. 并且该权重必须大于一个阈值，如果没有超过该阈值的权重，那么就只保留权重最大的边（与其它关键帧的共视程度比较高）
3. 对这些连接按照权重从大到小进行排序，以方便将来的处理
   更新完covisibility图之后，如果没有初始化过，则初始化为连接权重最大的边（与其它关键帧共视程度最高的那个关键帧），类似于最大生成树

我们看到大量重复的并且要保存下来数据的有2种，一种是MapPoint，一种是KeyFrame，路标点之间假设是独立的，路标点和位姿（关键帧）之间具备联系，位姿和位姿之间是关联的。

另外一个问题SLAM中的Data Association表示的是什么，这里最开始的认识就是路标点之间的数据关联，只有但那些点关联之后，我们才会直到，哪些点构成一个物体，或者一个目标，如果目标是移动的，我们也得到一个启发式的处理动态场景的方法。

地图

• vector<路标点>
• vector<关键帧>
• 对关键帧进行优化（BA）
• 删除/增加路标点
• 增加/删除关键帧

跟踪器（定位的过程）

在跟踪中初始化的时候使用的是单应矩阵和基础矩阵，在跟踪时候匹配当前帧和路标点，一个图像中的点，一个三维空间中的点，因此用的是PnP，优化Pose，路标点这里是假设为静止的，只有pose被优化。

地图（BA优化）

主要有2个待优化的内容，一是轨迹中位姿，另外一个是图中的路标点，不过很多时候路标进过几次优化之后就会不动了，那么这个时候就会使用一个pose graph来降低优化的规模，在ORB中 Covisibility Graph建模这些位姿，就是用路标点个数表明位姿节点的权重的一个图。
BA，这个东西一般是放在整个过程的最后，就是别人都折腾过（优化）一遍之后（紧着那些保证实时性的那些东西尽快的先搞出点东西来用着），把所有的东西放到一个优化器全部优化。这样做的好处就是优化过后的各部分都会精度提到，便于以后放心的使用这些东西，踢掉了不好的outliers，把网络之后的关系理顺。