（四）cartographer算法用于调试的参考解读
cartographer是一个复杂的系统，要想调试它首先需要对其内部工作原理有较好的理解，这篇文章试图给出cartographer各个子系统直观的概览还有它们的配置参数。如果你对cartographer非常感兴趣，建议看看这篇文章，虽然它只介绍了2D-SLAM，但是它严格的介绍了我们这里将要介绍的每一个概念，当然这些概念同样适用于3D-SLAM。
W. Hess, D. Kohler, H. Rapp, and D. Andor, Real-Time Loop Closure in 2D LIDAR SLAM, in Robotics and Automation (ICRA), 2016 IEEE International Conference on. IEEE, 2016. pp. 1271–1278.
概述
cartographer可以看成2个独立又关联的子系统。第一个叫做局部SLAM（有时我们也叫它前端或者局部轨迹生成器）。它的工作就是建立连续的子图。每个子图局部是一致的、没有误差的，但是也认识到局部SLAM随着时间误差逐渐累积的问题。绝大部分的局部SLAM的配置文件在这里可以找到：
2D：/cartographer/configuration_files/trajectory_builder_2d.lua
3D：/cartographer/configuration_files/trajectory_builder_3d.lua
另一个子系统叫做全局SLAM（有时我们也叫它后端）。它在后台线程中运行，它的主要工作是找到闭环检测的约束（通过扫描激光帧与之前建立的子图进行匹配）。它还可以可以使用其他传感器的数据与激光融合，得到全局一致性的结果（想想我们之前提到的相机landmark）。在3D中，它还可以修正重力的方向。绝大部分的全局SLAM的配置文件在这里可以找到：
3D：/cartographer/configuration_files/pose_graph.lua
简单一点说，局部SLAM就是建立每一个良好的子图，全局SLAM就是把各个子图正确的链接起来。
输入
由于过近过远的激光点存在测量误差及噪声，所以可以通过这者最小距离和最大距离限制输入的激光点数据，具体参数如下：
TRAJECTORY_BUILDER_nD.min_range
TRAJECTORY_BUILDER_nD.max_range
注意：
1、对于大于max_range的激光点，将用TRAJECTORY_BUILDER_2D.missing_data_ray_length 来替代它的距离，这样的做法是为了不让建图时因点探测过远出现近距离数据丢失的问题。
2、cartographer里的距离单位都是米。
由于存在激光运动畸变问题，cartographer里面每个激光点具有时间戳，然后根据imu等高频数据来矫正激光运动畸变问题，所以建议采集激光频率越高越好。
2d激光测量的方式，导致近距离的激光点比较密集，而远处的点比较稀疏，为了解决这个问题，我们使用了体素网格化下采样，这个方法很好理解，参考PCL几种采样方法，具体参数如下
TRAJECTORY_BUILDER_nD.voxel_filter_size
除此之外，cartographer里面还有另一种自适应体素滤波（采样）方法，其目的就是为了自适应计算合适的采样间隔，来获得指定的点云数量。具体参数如下：
TRAJECTORY_BUILDER_nD.*adaptive_voxel_filter.max_length //采样最大间隔
TRAJECTORY_BUILDER_nD.*adaptive_voxel_filter.min_num_points //采样后最小点云数量
IMU是个非常有效的数据源，因为它提供了精确的重力方向，它提供了每一激光帧的初始值，大幅度的减少匹配计算量（特别是3D）（我觉的这个在3D-SLAM比较明显，[带解决问题] 2D时感觉没有用到重力方向，反而用到的是imu的加速度来矫正激光畸变），所以无论2D还是3D，如果你有imu数据，一定要使用它。
[为什么？] 你会问如果我没有imu数据，2D-SLAM没有办法建图吗？答案是可以的，但是你要付出其他代价，就是打开
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
这一句的作用是在使用ceres做优化匹配前，先用CSM相关性匹配的激光位姿结果作为ceres优化匹配的初始值，这样的话就可以替代imu计算位姿初始值的作用，但是这样会增加前端的计算量，所以在使用时根据自身的情况酌情使用。其具体参数如下：
TRAJECTORY_BUILDER_2D.use_imu_data
TRAJECTORY_BUILDER_nD.imu_gravity_time_constant
局部SLAM
局部SLAM是通过每一帧激光与当前子图进行扫描匹配进行构建的，而在匹配前，通过位姿推断器（pose extrapolator）来给予每一帧激光匹配的初始位姿，而位姿推断器除了使用激光数据，也可以使用其他数据（如imu、odom等），位姿推断器实现了融合多数据的位姿更新。
而扫描匹配有2种方式：
- CeresScanMatcher匹配：使用ceres优化库构建优化方程来进行匹配，这种匹配适用于提供良好位姿初始值的情况，可以实现亚像素的匹配，速度快，缺点是如果初始值不理想，匹配也有可能不理想。
- RealTimeCorrelativeScanMatcher匹配（CSM匹配）：当你没有其他数据或者不信任其他数据时，它使用了一种与回环检测相似的匹配策略，并把匹配的结果作为CeresScanMatcher匹配的初始值，这种匹配的代价较高它会淹没掉其他传感器的作用，但是它在特征丰富的环境比较鲁棒，缺点计算量增加。
CeresScanMatcher匹配可以对输入数据赋予权重，这个权重是对测量数据的信任度，可以被看作静态的协方差。权重越大，cartographer将越偏向这种参数规则。这些权重包括：
TRAJECTORY_BUILDER_3D.ceres_scan_matcher.occupied_space_weight
TRAJECTORY_BUILDER_3D.ceres_scan_matcher.occupied_space_weight_0
TRAJECTORY_BUILDER_3D.ceres_scan_matcher.occupied_space_weight_1
TRAJECTORY_BUILDER_nD.ceres_scan_matcher.translation_weight
TRAJECTORY_BUILDER_nD.ceres_scan_matcher.rotation_weight
CeresScanMatcher匹配是使用谷歌的ceres来构建非线性最小二乘问题。这个模板匹配问题被构建为求解2帧激光数据之间的旋转矩阵参数。Ceres使用梯度下降和设置迭代次数来优化这个问题，Ceres可以根据你的需求来配置收敛速度。
TRAJECTORY_BUILDER_nD.ceres_scan_matcher.ceres_solver_options.use_nonmonotonic_steps //？？？
TRAJECTORY_BUILDER_nD.ceres_scan_matcher.ceres_solver_options.max_num_iterations
TRAJECTORY_BUILDER_nD.ceres_scan_matcher.ceres_solver_options.num_threads
RealTimeCorrelativeScanMatcher匹配可以根据你对传感器的信任程度来进行设置，它有一个搜索窗口（包含一个最大搜索距离和一个最大搜索角度），还有2个权重，分别设置平移和旋转的代价，意思是距离初始位姿越远，代价越大，可信逐渐降低。具体参数如下：
TRAJECTORY_BUILDER_nD.use_online_correlative_scan_matching
TRAJECTORY_BUILDER_nD.real_time_correlative_scan_matcher.linear_search_window
TRAJECTORY_BUILDER_nD.real_time_correlative_scan_matcher.angular_search_window
TRAJECTORY_BUILDER_nD.real_time_correlative_scan_matcher.translation_delta_cost_weight
TRAJECTORY_BUILDER_nD.real_time_correlative_scan_matcher.rotation_delta_cost_weight
为了避免一个子图中插入过多冗余的数据，这里设置参数只有满足2帧激光数据间隔一定角度、距离、时间时，这一帧激光数据才插入子图，具体参数如下：
TRAJECTORY_BUILDER_nD.motion_filter.max_time_seconds
TRAJECTORY_BUILDER_nD.motion_filter.max_distance_meters
TRAJECTORY_BUILDER_nD.motion_filter.max_angle_radians
一个子图内激光帧数是人为设定的，当一个子图积累够固定数目的激光数据后则一个子图生成完毕。一个子图内激光的数量不能太大，太大可能会产生位姿漂移，也不能太小，太小不利于后台的回环检测。具体数值需要人为调试来确定，参数如下
TRAJECTORY_BUILDER_nD.submaps.num_range_data
地图有2种存储格式：1种是概率图，1种是TSDF，参数如下：
TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.grid_type
概率栅格将空间划分成二维或三维网格，每个网格保存被阻塞的概率，概率根据hit和miss的权重来进行更新，具体参数如下：
TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.hit_probability
TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.miss_probability
TRAJECTORY_BUILDER_3D.submaps.range_data_inserter.hit_probability
TRAJECTORY_BUILDER_3D.submaps.range_data_inserter.miss_probability
2D和3D概率地图均可以在rviz上查看
全局SLAM
局部SLAM生成一个一个的子图，而全局SLAM负责通过优化的方式（SPA）在后台来重新调整各个子图的位姿使得形成一个完整连贯的全局地图。此优化负责更改当前构建的轨迹，以根据闭环检测来正确地对齐子图。
全局优化按照一定的规则频率进行，当检测到有一定数量的轨迹节点插入到地图中，则执行一次优化，参数设置如下：
POSE_GRAPH.optimize_every_n_nodes
注意：当POSE_GRAPH.optimize_every_n_nodes等于0时，相当于关闭了全局优化，从而可以专注于局部SLAM的状态，这往往是调试cartographer的第一步。
全局SLAM又可以被称作图优化，通过建立路径节点和子图之间的约束，然后优化这些约束，路径节点和子图是空间位姿，约束是空间位姿的旋转平移矩阵，用ceres的自动求导省去了自己求雅可比的工作。
注意：这些约束可以在rviz上显示出来，而打开
POSE_GRAPH.constraint_builder.log_matches
则可以查看约束的直方图形式的日志报告。
约束分为2种：非全局约束和全局约束
- 非全局约束，是建立与自身路径节点比较近时的约束，一般是与前一个子图和当前子图之间建立，目的是保持局部路径的连贯性。
- 全局约束，是当检测到有回环，建立起的约束，一般是新生成的子图与之前的路径节点之间建立，目的是保持全局路径的连贯性。
参数如下：
POSE_GRAPH.constraint_builder.max_constraint_distance
POSE_GRAPH.fast_correlative_scan_matcher.linear_search_window
POSE_GRAPH.fast_correlative_scan_matcher_3d.linear_xy_search_window
POSE_GRAPH.fast_correlative_scan_matcher_3d.linear_z_search_window
POSE_GRAPH.fast_correlative_scan_matcher*.angular_search_window
注意：实际上，全局约束不仅可以在一条单一的路线上进行，在多机器人多路线上同样可以。其实这个功能也不难理解，比如你建立了一部分地图，突然这个时候机器人没电了，那之前建立的地图岂不白费？当然不是这样的，你可以保存之前建立的地图，下次建图加载上一次的地图即可，这和多机器人多路线是一个原理，不过小编没试过，有时间试下。
为了限制约束的数量，cartographer限制一部分的路径节点建立约束，并通过一个约束采样因子来控制，不过，太小的因子可能造成缺失约束和无效的回环检测，太大因子会降低全局SLAM的效率而且无法进行实时回环。参数如下：
POSE_GRAPH.constraint_builder.sampling_ratio
回环检测需要用到激光匹配，而这个算法叫做FastCorrelativeScanMatcher，他被设计用来尽可能地实现实时检测回环，它使用一种”分支界定“（Branch and bound） 的方法来实现，其实类似与模板匹配使用图像金字塔的策略，先在低分辨率上进行搜索，然后逐渐在高分辨率上搜素，在2d代码中实现使用了递归的方法，短短20行代码。可能在3D上这个算法思路体现的会更加清晰。具体参数如下：
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.branch_and_bound_depth
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.branch_and_bound_depth
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.full_resolution_depth
一但FastCorrelativeScanMatcher找到了足够好的匹配（大于最小匹配值），它会再用Ceres Scan Matcher进一步再优化一下位姿（匹配结果）。
POSE_GRAPH.constraint_builder.min_score
POSE_GRAPH.constraint_builder.ceres_scan_matcher_3d
POSE_GRAPH.constraint_builder.ceres_scan_matcher
当cartographer运行后端优化时，Ceres通过多种残差来调整子图的位姿，每种残差被赋予不同的权重（又叫做代价函数），以此来调整不同数据来源的信任程度，是不是看到了协方差矩阵的影子。这些权重包括：全局约束权重（闭环检测=loop closure）、非全局约束（局部约束=matcher）、IMU加速度和角速度测量权重、固定框架权重（例如GPS系统），这些参数如下：
POSE_GRAPH.constraint_builder.loop_closure_translation_weight
POSE_GRAPH.constraint_builder.loop_closure_rotation_weight
POSE_GRAPH.matcher_translation_weight
POSE_GRAPH.matcher_rotation_weight
POSE_GRAPH.optimization_problem.*_weight
POSE_GRAPH.optimization_problem.ceres_solver_options
可以通过最大迭代次数来调节优化结果
POSE_GRAPH.max_num_final_iterations
IMU作为残差的一部分，默认情况下优化问题为IMU位姿提供了一定的灵活性，即可以把IMU和tracking_frame之间的外参作为优化变量来解算，ceres全局优化的结果用于外参矫正并被输出到log。如果优化结果不理想或者你相信自己外参，你可以设置这个变量为常数，参数如下：
（还没试过…）
POSE_GRAPH.optimization_problem.log_solver_summary
POSE_GRAPH.optimization_problem.use_online_imu_extrinsics_in_3d
在残差处理中，通过Huber loss函数来控制离群值（错误的数据）的影响大小，Huber loss函数是一种核函数，它通过一个Huber因子来调节，Huber因子越大，离群值（错误的数据）对整体的影响越大。
POSE_GRAPH.optimization_problem.huber_scale
一旦轨迹完成，cartographer将执行新的全局优化，并且通常比以前的全局优化进行更多的迭代。 这样做是为了完善cartographer的最终结果，通常不需要实时进行，因此大量迭代通常是正确的选择。
POSE_GRAPH.max_num_final_iterations
其实里面还有很多细节未说明，待补充。