Slam设计模式

对于“面向对象”的程序设计者来说，有本必备的书的叫做《设计模式-可复用面向对象软件的基础》。那么在slam的实现中有没有这一种或者几种可以总结归纳的设计模式呢？
我们在看一些Slam的书的时候，有些要么就是介绍很多状态估计的理论，要么就是介绍多视图几何的各种视觉理论。那么我们在看了这些理论之后，仍然无法下手，那么一个典型的好的Slam的代码的基本结构是什么样的呢？一个slam代码的基本结构是怎样的？有哪些基本要素？
一个自然的想法就是从图像中获取想要的定位的信息。在机器人移动的过程中相机捕捉到一个图像序列。在这个图像序列中机器人看到周围的景物是在按照一定的规律移动和变化的，那么景物移动的规律和机器人自身移动的规律有什么关系呢？这个关系可以根据空间变换来确定。
那么如何根据图像序列的内容提取景物的移动规律呢？可以跟踪图像上景物的一些点，图像很多点的统计规律可以表征景物的移动规律。这就是提取特征点进行跟踪或者直接使用灰度点跟踪的意义所在，根据我们使用的点的方式不容，可以看到两种不同的slam前端方法——直接法和特征法。
那么如何实现这样的跟踪器呢？slam算法在读到一张图像之后，首先把当前图像的点和前一帧图像的点对应起来，那么需要多少个点对应呢？怎么实现点对应呢？基于特征的方法是提取图像中的特征点，用于匹配不同图像中相同的点，这是一种思路，常用的有SIFT ORB等。另外一个思路是由于机器人的运动相对于图像的帧率较慢的时候，图像的内容变化较小，因此在局部搜索可以找到上一帧图像的点的位置，这就是直接法的思路。
我们这里引入一个Frame结构，为了实现上述的跟踪器，第一步是读取图像，通过图像的数据估计当前帧对应的位姿状态。场景中相同的点在不同的视角中构成一个基础矩阵的关系，一个图像帧的图像平面和另一帧的图像的平面之间构成单应矩阵的关系。使用最小二乘法通过图像点可以计算得到单应矩阵和基础矩阵，使用矩阵奇异值分解（SVD）可将单应矩阵或者基础矩阵分解恢复出位姿变化。这些内容的实现可以放到Frame结构，也可以像ORB那样专门放到一个Initializer里边。Frame需要和Map完成互动。除了需要计算当前帧的位姿，还要完成特征点的三维重建。有的方法使用了一个深度滤波器，如SVO，LSD中考虑了极线与深度的夹角，这样会得到一个半稠密的地图。因为稠密的地图点想要试试完成一般是需要使用GPU来完成的。图像的上的特征点和空间中的三维点如何对应呢？常用的方法是PnP。PnP是已知三维点计算相机位姿，三维重建是已知相机位姿计算三维点。
PnP的计算中需要知道相机的内参数，这里引入第二个结构—-相机模型Camera，他保存并管理相机的参数，进行一些图像空间到世界空间的转换，完成图像点（关键特征点）的去畸变。
如何解决累积误差问题？常用的方法是使用路标点和当前图像点进行匹配估计相机的运动，这时候需要存储这些路标点的描述子。另外一方面在LSD-SLAM, SVO等方法中，是怎样存储路标点的呢？这里引入一个结构叫做Mappoint。
如何评价一个路标点的好坏程度？如何保存并管理这些路标点呢？路标点的添加删除等管理工作可以放入另外一个结构Map中进行。地图中以何种形式保存这些路标呢，一种方法是路标放在图像帧里，作为关键帧KeyFrame进行保存和提取。关键帧串起来成为一个地图。这样做的好处就是我们使用了关键帧继承了帧的性质，便于以后特征点匹配。在前边累计相机运动的时候，参考帧是前一帧图像，这里的参考帧是关键帧的图像。关键帧是相机运动过程中保留了重要信息的最小集合中的元素。理论上关键帧是没有重叠的。后端优化的对象主要是关键帧。
那么我们换一个角度来看问题，从时间轴上，我们关心的有两个状态，一个当前的时刻，另外一个以前的时刻。对于当前的时刻我们要保持实时性。对于以前的时刻我们要通过他来提高精度。这就是为什么要分开来做前端和后端的问题。 事实上，优化并非仅仅存在于后端，优化存在于前端的运动估计中(基础矩阵计算，于运动估计的计算)，后端中关键帧的优化（BA）。这些优化中形成矩阵结构H是稀疏的（对应图网络），可以显式的利用图优化表示。H的稀疏性为图优化方法的SLAM提供了实时性的保证。这里可以引入一个结构Optimizater，用来完成上述内容中提到的各种优化工作。我们这里总结一下可以优化的步骤是那些呢？
• 三维点（点的深度）的优化（三角测量）
• 后端的关键帧中的位姿和路标点的BA（关键帧的边缘化）
• 初始化过程的单应矩阵和基础矩阵的优化
• PnP计算相机位姿
注意
有些VO的算法中由于后端很小，所以把后端的内容一起移到了前端，然后每一帧都做一次BA，这样要求H矩阵（BA中的被优化结构）一定要保持稀疏性。
维护一个地图的另外一个好处就是即使当前跟踪的某一帧丢失，之后跟踪上的帧仍然可以恢复出运动。这样的前提是一个不断更新的地图，而且这个地图是正确的。这样的地图可以是局部的也可以全局的。全局地图的规模较大（更像SLAM，可以完成闭环），计算量比较大，因此适用于一些要求精确定位的地方，对于一些计算能力弱的场合适用于局部地图（更像一个VO，不能做闭环）。在局部地图中，考虑需要丢掉一些之前的帧，这个过程叫做marginalization(边缘化)。
跟踪器的状态有几种，1. 初始化，顺利跟踪，丢失。因此我们还需要一个初始化的过程，这个过程中有哪些内容？对第一/二帧图像的匹配，单应和基础矩阵的计算。这个其实在Frame结构中涉及过。那么可以构建一个Initializer，完成相应的工作。
到这里，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。
mapping其实也很忙，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，把网络之后的关系理顺。