Cartographer代码建图实现流程解析

1.整体代码构成

在整体的原有框架中代码主要分为两个部分：cartographer和cartographer_ros。
cartographer主要负责处理来自雷达、IMU和里程计的数据并基于这些数据进行地图的构建，是cartographer理论的底层实现。建图过程主要分为前端Local累加式建图与后端Global位姿图构建与回环匹配。
cartographer_ros则基于ros的通信机制获取传感器的数据并将它们转换成cartographer中定义的格式传递给cartographer处理，与此同时也将cartographer的处理结果发布用于显示或保存，在低层算法和机器人操作系统ROS之间提供了交互的接口。

2.底层算法构成

基于单线激光雷达的cartographer建图过程可简要表示如下图:

其中输入项中Range Data激光数据为必须，其余均为非必须。
整体的建图可分为Local前端部分和Global后端回环部分。

2.1 Local部分:

Local SLAM主要负责地图的实时累加式建立，主要在local_trajectory_builder_2d中实现。该部分通过一帧帧的Laser Scan建立并维护一系列的Submap，即一系列小规模的栅格地图。当再有新的Laser Scan时，会通过Fast-CSM和Ceres Scan Matching的方法将其插入到子图中的最佳位置。但是submap的累加式建图会产生误差累积的问题，该问题由Global部分解决。
前端Local部分输入的传感器数据可以有四个：Range Data(激光雷达，摄像头等)，Odometry Pose(里程计数据)，IMU Data，FixedFramePose(当前版本尚未最终实现)。
里程计与数据与IMU数据共同进入PoseExtraPolator(姿态推算器)，得到里程计与IMU得到的位置估计，然后推算出下一时刻的位置姿态，然后给到Scan Matching中作为扫描匹配的初值。此处里程计和IMU的数据并不是必须的。
在LocalTrajectoryBuilder的AddRangeData中，Range Data数据经过体素滤波，之后将各个点存入returns或者misses集，然后进入scanMatching，如果使用了IMU，则将姿态推算器获得的当前姿态推算作为优化初始值。这里ScanMatching使用基于ceres优化的方法获得观测最优位置估计，经过Motion Filter滤除运动过小的帧，之后调用InsertIntoSubmap()函数插入以构建submap。

2.1.1 AddRangeData()

在AddRangeData()中，对一帧点云的每个点数据，筛出小于指定距离的点，并逐个记录为returns；对于大于指定距离的点，将该方向上一定距离的某点记入misses点集。此处可将多帧数据一并存入并在后续工作中作一帧处理，默认仅累计一帧的数据。之后对这一累积帧数据进行体素滤波。

2.1.2 TransformToGravityAlignedFrameAndFilter()

在该函数中，输入的点云数据经过修剪筛去了z轴阈值之外的点。之后通过VoxelFilter()分别对returns点集和misses点集进行体素滤波。在每个体素内仅保留一个点。

2.1.3 AddAccumulatedRangeData()

经过体素滤波的点云在该函数中完成。从姿态推算器中获取当前点云帧的姿态初值，进入ScanMatch()的模块以优化点云帧的位姿。匹配完成后通过InsertIntoSubmap()把点云帧插入子图。

2.1.4 ScanMatch()

首先对点云进行一个自适应体素滤波，然后获得当前活跃的两个子图中的较旧的一个子图，之后当前帧将与其进行匹配以优化当前帧位姿。
$★$Real-time CSM
首先通过real-time CSM对激光帧数据在submap的栅格地图上进行一次粗匹配，产生一个大体较好的位姿值并返回一个得分数。
Real-time CSM的核心思想来源于论文Real-time correlative scan matching，文中提出了一种概率驱动的扫描匹配方法。
$$p(z_t|x_t,m_{t-1})$$
以上称为观测模型。
其中，$x_t$，$x_{t-1}$就是当前时刻和上一时刻的位姿，前者为未知的带求解的量，$z_t$为当前scan里的所有数据的集合即雷达观测数据，相当于是当前一帧雷达数据中的所有点，包括角度和距离；$m_{t-1}$为现有的地图信息。
雷达观测数据里包含了很多点数据，而这些数据相互之间并没有关联，所以可以假设他们是完全独立的。由此可以将式子分解：
$$p(z_t|x_t,m_{t-1})=\prod _{i}p(z_t^i|x_t,m_{t-1})\propto \sum _i\log p(z_t^i|x_t,m_{t-1}))$$
上面的式子将整个一帧观测数据的概率拆分成了当前帧的数据的每一个点的概率和。
在算法的实现中，程序采用以下方法：
a.在上一时刻位姿的周围设定一个搜索窗口Search Window，将激光帧在一个范围内按设定的旋转步长生成一系列旋转后的激光帧，将这些候选帧映射入栅格。
b.之后再将这些激光帧按一定步长在x、y轴上平移，进一步生成一系列的候选帧。这些候选帧覆盖了该窗口内的所有可能位姿。
c.最后遍历这个候选的子集，计算每个候选与地图匹配的概率和，找到那个概率和最高的就是我们要找的当前机器人的位姿。
历遍所有可能位姿的做法是暴力的，然而由于窗口较小，实时性没有受到很大影响。

$★$Ceres scan match
之后将该位姿再进行一次与子图的匹配ceres_scan_match，以获得最终的进一步优化位姿。最后返回优化的位姿。采用最小二乘法解决这个问题：
$\underset{\xi }{\mathrm{argmin}}{\sum }_{k=1}^K(1-M_{smooth}(T_{\xi }{h}_{\xi }){)}^2$ (CS)
这里scanMatching方法对每个栅格点的概率进行双三次样条插值平滑，然后构建优化问题。式子中M即概率，概率值为0到1的一个数字，因此M这项越大，1-M就越小，这个优化函数目标是获取一个概率最高的位置。
因为这是一个局部优化问题，所以需要用imu提供一个好的角度初始值。如果没有，也可以用高概率的scan matcher 或者pixel-accurate来获得值作为初始值，但是计算很复杂。

2.1.5 InsertIntoSubmap()

首先MotionFilter检测此次位姿运动的大小，当小于某阈值时则不加入submap。之后使用InsertRangeData()根据优化好的位姿将点云帧分别插入当前活跃的两个子图中。
submap栅格地图如下图所示，其中打叉的灰色点为hit，无打叉的灰色点为miss。

更新子图时将每个hit的点与原点连线上的点全部设置为miss，根据以下公式对子图上hit的栅格概率odd进行更新。对于miss的网格点的修改同理。
$odds(p)=\frac{p}{1-p}$
$M_{new}(x)=clamp(odd{s}^{-1}(odds(M_{old}(x))\cdot odds(p_{hit})))$
其中clamp是区间限定函数，大于max，返回max;小于min,返回min。该更新公式可由贝叶斯法则推导获得。
依据以上方法，新scan被加入当前激活的两个子图。

2.2 Global部分:

Global的回环优化每隔一定数量激光帧以后进行。因为一个submap是有少数的几个连续scan点集生成的，所以误差很小，子图内部不需要继续优化。如果检测到当前的扫描帧scan附近存在位置足够接近的历史子图submap，则scan-match可以将子图和扫描帧进行匹配，其中粗匹配过程在搜索窗口内使用fast-CSM分支定界获得粗匹配结果，如果找到好的匹配，则使用（CS）式进行精细匹配调整，之后其作为约束添加到优化（SPA）中。

2.2.1 global_trajectory_builder

在Global轨迹构建器中组织起了整体SLAM建图过程，其内部将建图分为了2d与3d两部分。在2d建图时，构建器通过CreateGlobalTrajectoryBuilder2D()构建了2d情况下的整体建图过程。
构建地图时，程序首先构建了前端建图的local_trajectory_builder_(Local轨迹构建器)和用于回环全局优化的pose_graph_(位姿图)。对每一帧激光帧，程序首先调用了Local轨迹构建器的AddRangeData()方法，将激光帧逐帧加入一个个子图，以完成实时累加式地图的建立（2.1）；之后构建位姿图，使用AddNode()，把前述匹配好的激光帧作为Node加入位姿图中进行后续全局匹配。
位姿图的构成如下图所示：

图中Node表示各个激光帧，Submap为各个子图，虚线表示构建的位姿约束。其中与Submap m相关的约束表示激光帧在Submapm中检测到了匹配并构建了新的约束（回环）。
全局回环优化可分为位姿图的构建以及位姿图的优化两大部分。

2.2.2AddNode() in pose_graph_2d

在 pose_graph_2d中的AddNode()实现将激光帧与附近其他submap扫描匹配形成约束。这些约束在后面的位姿图优化时将组成位姿图的“边”参与到优化的过程中去。在pose_graph中，为了保证位姿图相关概念名词的一致性，故将参与约束构建的激光帧称为TrajectoryNode (轨迹节点)，简称Node (节点)。
在该部分的实现中，AddNode()首先将输入的Node加入到pose_graph维护的trajectory_nodes_变量里。通过检测之前被Node插入的submap是否曾被pose_graph观测到，可以帮助pose_graph内部维护submap状态的队列submap_data及时进行更新。之后，调用ComputeConstraintsForNode()开始计算关于该Node的约束Constraint。

2.2.3 ComputeConstraintsForNode()

在该函数中计算与新添加的这个节点相关的所有约束关系并优化。
调用optimization_problem_的AddTrajectoryNode(),该节点被加入到优化问题中进行优化。
对于节点之前插入两个submap的过程，可以认为天然的形成了Node与前后两张submap的两条约束，所以之后直接将这两条约束加入到pose_graph中维护约束的队列constraints_里。
之后，历遍历史中的 submap，通过ComputeConstraint()计算新的 Node 与每个 submap 的约束。
如果通过标记发现有新的刚结束构建的 submap，则需要将其状态设置为finished，并调用ComputeConstraintsForOldNodes()为他计算新的 submap 和所有旧的节点的约束。
在每次调用函数加入节点时都会进行计数，以保证每隔若干个节点进行一次全局优化。当计数达到设定值时，调用DispatchOptimization()进行位姿图优化计算。

2.2.4 ComputeConstraint()

在该函数中计算一个node和一个submap的约束。输入变量即为Node和submap的id
首先需要确认该submap已经结束构建，对未结束构建的submap不进行约束计算;
（此处通过MaybeAddGlobalConstraint似乎可以实现全局定位）
对于初始位姿差距过大的node与submap，直接跳过约束的计算。
在约束计算的层层调用与各种条件判别中，较为重要的即为基于分支定界BBA的粗匹配和基于Ceres scan match的精匹配。

2.2.5 Fast- CSM&分支定界Branch And Bound

在全局匹配激光帧与子图时，由于搜索窗口更大，匹配次数更多，纯粹使用前述Real Time CSM暴力搜索十分消耗时间。Fast- CSM使用分支定界的办法加速了这一过程。
分支定界法（branch and bound）是一种求解整数规划问题的最常用算法。把全部可行的解空间不断分割为越来越小的子集（称为分支），并为每个子集内的解的值计算一个下界或上界（称为定界）。在每次分支后，对凡是界限超出已知可行解值那些子集不再做进一步分支。这样，解的许多子集（即搜索树上的许多结点）就可以不予考虑了，从而缩小了搜索范围。
分支定界法深度优先地查找最佳值，减小了计算所有情况的计算开销。
在Fast- CSM中，根据分支定界的思想，地图通过分辨率折半的过程被分作了多层，从最上层（最粗分辨率）开始用Real Time CSM的方法计算粗匹配最优匹配解。
在每层分辨率下，待匹配的Node根据给定的搜索窗口与步长创建一系列经过不同旋转、平移的候选匹配集。每一个候选位置的Node分别与当前分辨率下的submap进行匹配，获得每一个候选位置匹配的得分并进行排序获得得分最高的位置候选。
之后在更低一层的submap上对最佳位置附近进行展开，再次进行候选集生成、逐个匹配获取最佳值的过程。通过分支定界的方法最后找到最细分辨率下最佳的候选相对位姿。

该图为转载侵删

2.2.6 Ceres_scan_match

与Local部分相同，粗定位之后，调用Ceres的非线性优化对粗匹配的结果进行微调：
$\underset{\xi }{\mathrm{argmin}}{\sum }_{k=1}^K(1-M_{smooth}(T_{\xi }{h}_{\xi }){)}^2$(CS)
这里scanMatching方法同样对每个栅格点的概率进行双三次样条插值平滑，然后构建优化问题。优化后建立的约束被加入到pose_graph的constraint中，等待全局优化。

2.2.7 全局最终优化DispatchOptimization()

在全局优化中主要调用了optimization_problem_的Solve()对整个位姿图进行匹配。
在优化中，将节点信息子图信息（节点）和约束信息添加进ceres优化问题，随后调用优化库进行全局优化求解。
通过前述的Node与Submap之间的约束计算，约束图被逐渐构建出来。图中每个节点Node（激光帧）都与其插入的子图相连，若存在回环约束则该Node还与与之匹配的Submap存在约束连接。稀疏图优化有以下公式：
$\underset{{\Xi }^m,{\Xi }^s}{\mathrm{argmin}}\frac{1}{2}{\sum }_{ij}\rho (E^2({\xi }_i^m,{\xi }_j^s;{\Sigma }_{ij},{\xi }_{ij}))$ (SPA)
式中优化的变量是子图位姿和激光帧位姿，优化的目标是在每个子图位姿和激光帧位姿间最小化协方差和相对位姿的残差。
经过优化后每个激光帧和子图的关系都得到了修正，尤其是在回环时当构建了激光帧和很早的子图的约束时，全局优化可以有效解决累积误差带来的问题。