cartographer原理简述

参考:https://blog.csdn.net/weixin_36976685/article/details/84994701

Cartographer 能产生一个精度为5cm的2D栅格地图。

1.前端

Cartographer在前端匹配环节区别与其它建图算法的区别主要是使用了Submap这一概念，每当或得一次laser scan的数据后，便把此时所在位置看做一个节点，与当前最近建立的submap去进行匹配(前端的scan match)，得到一个最优位姿，用来作为ceres的初始化位姿initial_ceres_pose。前端的scan match策略为:scan_matching::RealTimeCorrelativeScanMatcher,

前端scan match策略简介

/** 首先由PoseExtrapolator根据imu和里程计通过滤波的方式预测一个初始姿态pose_prediction，然后在这个初始姿态附近放一个搜索框（注意，每一个角度也要枚举）* 对搜索框中的每一个（x,y,theta）进行枚举，打分score。* 打分机制：********************************************************************************************* 首先将原始点云进行体素滤波，然后将点云转换为栅格坐标的点云，然后运用（x,y,theta）将点云进行旋转，旋转后点云中的每一个点* 所落入的栅格都有一个occupied概率值，将这一个点云的所有点得到的概率值相加，然后除以点云的点个数，然后再乘以一个因子* 得到最终的打分结果* 得分最高的搜索框中的位姿（x*,y*,theta*）作为最优搜索位姿* ***************************************************************************************************** 这个最优位姿被叫做initial_ceres_pose，以后要用来作为ceres优化的初始位姿
*/

根据trajectory_builder_2d.lua文件中的

use_online_correlative_scan_matching = false

我们可以知道，默认是不需要前端scan match的。那么initial_ceres_pose就是简单的用里程计和imu的滤波得到的pose_prediction。

用前端scan match策略或者直接里程计和imu的滤波得到了ceres的初始优化位姿后，当然是使用了ceres库进行match优化,应该是仅仅优化位姿而不优化laser scan。优化得到位姿的同时在不断插入新数据帧的同时该Submap也得到了更新。一定量的数据组合成为一个Submap，当不再有新的scan插入到Submap时，就认为这个submap已经创建完成，接着会去创建下一个submap。

因此你可以通过前端，得出节点与submap的相对位姿。然后submap在当前轨迹中的位姿是submap建立的时候一开始第一个节点相对于轨迹坐标系的位姿。二者相乘，可以得到前端的最终结果————节点的map位姿。即节点相对于节点所在的轨迹坐标系的位姿。注意，是在他自己那条轨迹、即自己那个map中的坐标哦。

然后又知道轨迹坐标系在世界坐标系中的位姿，因此可以得到某个节点相对于世界的位姿，即绝对位姿。

解释一下上边，我喜欢把map坐标系作为轨迹坐标系，因为一个轨迹对应一个map嘛。

2.回环检测

Cartographer通过创建大量的submap来实现大场景建图，submap在短时间内的准确度是可靠的，但长时间会存在累积误差，为了消除累积误差，需要通过回环检测来构建回环,最终是为了优化所有submap的位姿。

首先回环优化，我们需要检测到回环，再进行优化。这里为了减少计算量，提高实时回环检测的效率，Cartographer应用了**branch and bound(分支定界)**优化方法进行优化搜索，如果得到一个足够好的匹配（即Tij），已经检测到了回环的存在。

我们从建立约束讲起。由于回环检测是一种全局约束。而其实后端同样构建了局部约束。

后端的匹配策略为：scan_matching::FastCorrelativeScanMatcher，详细介绍一下：

首先一个节点进来，我要计算它与其他子图间的约束。

因此我对此前所有轨迹中建立的所有子图进行循环，计算这个节点与循环中的各个子图的约束，根据节点和循环中的各个子图之间的情况不同，可能构建的是全局约束，也可能是局部约束

（1）局部约束：节点和子图在同一条轨迹中，或二者分别在两条轨迹中，但是节点距离这两条轨迹之间的上一次全局约束不远，则进行局部约束。

前端我们可以得到节点相对于世界的位姿，也可以得到某个子图的世界位姿，因此可以得到这个节点在这个子图中的位姿。也就是这个节点相对于这个子图的相对位姿。把这个位姿称为初始位姿1。为什么要用世界坐标系作为桥梁是因为这个子图和这个节点并不一定在一个map中，即不一定在同一条轨迹坐标系中。然后用submap在自己那条轨迹坐标系中的位姿乘以初始位姿1，得到初始位姿2，即此节点在此子图所在的大map中的位姿。这样，初始位姿2才能成为分枝定界方法的真正初始位姿。

在初始位姿2附近，利用分枝定界方法，获取一个最优位姿。

分枝定界的树结构一共七层，序号从上到下的索引分别为6-0。分枝定界加快回环检测的速度。
分枝定界是节点的位姿在submap中的分枝定界，而不是在submap所在map中的分枝定界

分枝定界求出的位姿被称为初始位姿3，这个初始位姿3及其携带的点云要被输入ceres与此子图进行优化匹配match。
注意：只是这个节点和这个子图之间的局部优化匹配，而不是全局位姿图的优化。

最终得到ceres的输出结果，即节点在map中的最优位姿。然后这个结果经过map到submap的转换，最终得到节点在submap中的相对位姿**。这个位姿并不是我们所说的回环边，其实就是一个普通约束，子图和节点的普通边

以上约束叫做：局部约束。节点和submap离得并不远。

（2）全局约束：如果有全局定位采样脉冲的话,则进行全域上的约束匹配——需要节点和子图二者不在同一轨迹，并在时间上和之前的全局约束关系相隔较远，才能触发采用全局约束。

注意，全局约束才构造回环边

前面说过，我们在建立局部约束的时候，使用了初始位姿2作为分枝定界的初始位姿，再其附近进行局部范围搜索。

但是现在建立全局约束的时候，直接在一个超大的范围内进行分枝定界搜索，并不需要计算一个特殊的初始位姿2，而直接把初始位姿设置为地图limits的中心点，可以理解为map的中心点。（因为carto中一条轨迹的map就是用map_limits表示的）

然后在这个超大的范围内进行分枝定界，通过此节点所携带的点云与此submap的匹配程度，进行分枝定界求解。找到这个map中最能与此submap匹配的位姿。

即搜索出此节点的初始位姿3。

然后与局部约束相同，将初始位姿3经过节点与submap之间的ceres优化，最终得到回环约束位姿。

通常说的回环边有两类（里程计边、scan match边），这一类属于scan match得到的回环边。

总结与补充

通过上述两种方法，即全局约束和局部约束，构建了一些scan match边。不管是局部还是全局，都是实现的某个节点与某个submap之间的连接。

补充一下搜索范围：
‘’’
局部搜索的范围：±7m，±30°
全局搜索的范围：10^6栅格范围（乘以分辨率0.05为长度），±180°
‘’’

提出一个小疑问：局部约束算回环吗？欢迎质疑

3.后端优化

对于全局优化来说，节点和节点之间是里程计边，Tij可以认为是里程计的测量数据，构成图优化的measurement。节点和某些子图之间的局部约束也可以当做是约束边，是一种scan match边。但是对于回环边，就是上述回环检测章节所讲述到的scan match边，也是节点和submap之间的约束。

详细介绍：参考网址

如下图所示有两类顶点，三角表示子图submap，圆圈表示节点。
由于传感器的一次扫描数据可能插入多个不同的子图，所以同一个节点可能与多个子图之间存在一定的匹配关系（即节点在子图中的相对位姿），这个匹配关系就是所谓的约束。

之前说过，具有约束关系的节点和子图不一定是在同一条轨迹中的。
我正在tracking的这条轨迹中的节点可能会和其他轨迹中的submap产生约束联系，加入最终的global位姿优化。
这些约束可以被分为局部约束和全局约束

其实节点之间也不是孤立的，它们都是在上一个节点的基础上累加一些运动得到的。这些运动的测量就体现在了里程计以及IMU的积分定位、前端的扫描匹配等方面。各个节点前后串连起来就得到了一条轨迹。

自从上次后端的全局优化之后，每向优化问题中加入90个节点以后，进入后端的全局优化过程。
这就是隔一段时间进行一次优化的机制

submap和node之间有约束、node和node之间的联系是什么呢？：

（1）如果没有里程计数据，则node和node之间的相对位姿是经过local slam获得的，因为local slam获得了local pose，即相对于地图map的相对位姿，所以用节点1->map->节点2构建两个节点的相对位姿。

（2）如果有里程计数据，则两个节点之间还可以加入里程计数据作为相对位姿约束。

node和子图、node和node的约束都弄明白了以后，使用ceres进行全局优化。

这与我们常说的位姿图优化确实有一点小小区别。

当然可以同时优化位姿和路标点，只需要在backup_2d.lua文件中将use_landmark参数设置为true即可。