主要面向毫米波slam和视觉slam

现在我们能做到比较好的实时追踪和建图。大部分研究集中在Tracking上面，不过这种方式在无闭环时必定有累计误差，我个人经验是在0.5%-1%这个数量级，也许个别方案在个别数据上看起来好一些，不过视觉肯定比激光要差一些，比仔细标定的轮速计也差一些，也比较容易受干扰，但好在是一个独立的系统，和别的能够互相补充。有回环可以消累积误差，前提是得回来，自动驾驶很多场景是车一直往前开，基本不回去。

TODO OR TOLEARN

八叉树和四叉树   half-ok

特征点法和直接法的概念，以及对应的优缺点。

关键帧是什么？有什么用？如何选择关键帧

单目视觉slam中尺寸漂移是怎么产生的？有什么解决办法

直方图滤波    ok

高斯求和滤波  

 FastSlam   1/3 ok

 IF SELF 滤波 

 iSAM2 

TORO 

谷歌Cartographer

 Cartographer在前端匹配环节区别与其它建图算法的主要是使用了Submap这一概念,每当或得一次laser scan的数据后，便与当前最近建立的Submap去进行匹配，使这一帧的laser scan数据插入到Submap上最优的位置 ，在不断插入新数据帧的同时该Submap也得到了更新。一定量的数据组合成为一个Submap，当不再有新的scan插入到Submap时，就认为这个submap已经创建完成，接着会去创建下一个submap。

通过scan matching得到的位姿估计在短时间内是可靠的，但是长时间会有累积误差。因此Cartographer应用了回环检测对累积误差进行优化。所有创建完成的submap以及当前的laser scan都会用作回环检测的scan matching。如果当前的scan和所有已创建完成的submap在距离上足够近，则进行回环检测。这里为了减少计算量，提高实时回环检测的效率，Cartographer应用了branch and bound(分支定界)优化方法进行优化搜索。如果得到一个足够好的匹配，则会将该匹配的闭环约束加入到所有Submap的姿态优化上。

  回环检测即是一种匹配过程，即当获得新的scan时，在其附近一定范围搜索最优匹配帧，若该最优匹配帧符合要求，则认为是一个回环

分支定界：

主要思想：把全部可行的解空间不断分割为越来越小的子集（称为分支），并为每个子集内的解的值计算一个下界或上界（称为定界）。在每次分支后，对凡是界限超出已知可行解值那些子集不再做进一步分支。这样，解的许多子集（即搜索树上的许多结点）就可以不予考虑了，从而缩小了搜索范围。

ScanMatch成员函数基于submap已有的laser fan估计当前laser fan在submap中的位置。

Cartographer的重点内容是融合多传感器数据的局部submap创建以及用于闭环检测的scan match策略。重点内容对应的实现是：1）基于UKF的多传感器数据融合对应cartographer/kalman_filter目录下的文件；2）scan match策略对应cartographer/mapping_2d/scan_matching目录下的文件。

直方图滤波 又叫 栅格定位

原理

从一维开始说起:

机器人现在不清楚自己处于哪个格子

假设一个机器人在一个一维的世界里，具体假设场景[R,R,G,R,R]。其中R代表红色，G代表绿色。

1.首先机器人为所有特征点给一个平均概率，代表最混乱的情况。

所以机器人现在全局表是[0.2, 0.2, 0.2, 0.2, 0,2]，代表机器人有0.2的概率处在对应的位置。

2.当机器人感知到对应特征的时候，会添加一个滤波器

机器人每感知到一个特征就会为所有的位置添加一个滤波器，首先先定义实际值与测量值相同的概率和不同的概率pHit = 0.8（预设），pMiss=0.2预设，

原始状态是[0.2, 0.2, 0.2, 0.2, 0.2]，当感知到R特征的时候，就为所有R的位置添加0.8的滤波器. 0.2*0.8 = 0.16, 0.2 * 0.2 = 0.04  如：

机器人感知到R特征（红色特征）就为所有R的特征增加0.8的滤波器，也即所有R特征的位置现在的概率为0.2(初始值)*0.8.   同样，所有不是R特征的位置现在的概率为0.2（初始值）*0.2（测量出错的概率）

然后进行归一化的处理。 （因为现在世界上所有格子的总概率不为1了）

同时再考虑一个情况，机器人向前走了一步，但实际他可能超调（走了两步），可能不足（没走）：

所以现在为机器人的运动定义一个概率，这实际上就是机器人从S状态使用A动作后进入S'状态的概率

假设现在是[0.6, 0, 0, 0],机器人向前走两步，则 0.1 * 0.6 = 0.06， 0.8 * 0.6 = 0.48,结果是[0, 0.06, 0.48, 0.06]，最后归一化处理。

最后联合测量的更新和运动的更新，得到各个网格的概率分布，最大的一个就是认为的机器人的当前位置。

缺点是，如果栅格划分很细，则计算量很大。如果划分粗糙，则离散化时信息丢失会影响滤波器表现

FAST SLAM 

实际上就是粒子滤波SLAM 称做Fast Slam的原因在于，与基于 Kalman 滤波的 SLAM 不同, RBPF SLAM 采用粒子 来 表 示机器人的位姿,而环境特征的估计依然利用 EKF 解析计算。由于这 种 分 解 , RBPF SLAM 的 计 算 复 杂 度 为 O(NM) ,其 中 N , M 分 别 为 所用粒子的数目及特征个数,如给定粒子数 N , RBPF SLAM 的 计 算 复 杂度与特征个数 M 成线性关系,而传统 EKF SLAM 的复杂度为 O(M) ,因此 RBPF SLAM 也被称为 FAST SLAM , 这种算法降低了计算复杂度 

UPF-IEKF FastSLAM 2.0 算法采用 UPF 估计机器人的路径,地图估计则采用 IEKF 算法.UPF 算法使粒子向后验概率高的区域运动,提高了估 计 精 度 同 时 降 低 计 算 复 杂 度 ; IEKF 算 法 通 过 迭 代 观 测 更 新 过 程 来提高估计精度。综合考虑估计精度和计算复杂度, 认为 “ UPF-IEKF ” 是一种更合理的 FastSLAM 算法滤波架构,UPF-IEKF FastSLAM 2.0 ( n=2 )算 法 是 FastSLAM 2.0 系 列 算 法 中 最 理想的算法