Algorithm walkthrough for tuning（调优算法演练）

调优算法演练

cartographer是一个复杂的系统，对其进行调优(调整配置参数使其适应特定场景)需要很好地理解其内部工作原理。这里尝试给出一个直观的概述和介绍，包括cartographer使用的不同子系统及其配置值。如果您感兴趣的不仅仅是对cartographer的介绍，那么您应该参考cartographer论文。论文虽然只描述了2D SLAM，但它严格定义了这里描述的大多数概念。这些概念通常也适用于3D。

论文信息：
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看作两个独立但相关的子系统。

第一个是local SLAM(有时也称为前端或局部轨迹构建器)。它的工作是构建一系列的submap。每个submap都是局部一致的，但是随着时间的推移，会出现漂移。

大部分的local SLAM参数配置选项，可以在：
install_isolated/ share/ cartographer/configuration_files/ trajectory_builder_2d.lua对于2D；
install_isolated/share/ cartographer/configuration_files/ trajectory_builder_3d.lua对于3D；
中找到。(下文采用 TRAJECTORY_BUILDER_nD来代表2D和3D中共有的选项)

另一个子系统是global SLAM(后端)。它在后台线程中运行，其主要工作是查找环闭约束。它通过对scans-submaps扫描匹配来实现这一点。它还合并了其他传感器数据，以获得更高层次的视图并识别最一致的全局解决方案。在3D中，它还试图找到重力的方向。它的大多数选项可以在install_isolated/share/cartographer/configuration_files/pose_graph.lua中找到.

在更高的抽象上，local SLAM的工作是生成良好的子映射，而global SLAM的工作是将它们最一致地连接在一起。

输入

测距传感器（例如：LIDAR）提供多个方向的深度信息。 但是，某些测量与SLAM无关。 如果传感器部分被灰尘覆盖，或者扫测到机器人的一部分，则测得的某些距离可被视为SLAM的噪声。 另一方面，远距离测量值也可能来自不希望的来源（反射，传感器噪声），并且与SLAM也无关。 为了解决这些问题，cartographer需要首先应用 bandpass filter(边界滤波器)，将距离范围值保持在一定的最小和最大范围之间。 这些最小值和最大值应根据机器人和传感器的规格进行选择。

TRAJECTORY_BUILDER_nD.min_range
TRAJECTORY_BUILDER_nD.max_range

注意：
在2D模式下，cartographer用TRAJECTORY_BUILDER_2D.missing_data_ray_length替换了比max_range更远的范围。 它还提供max_z和min_z值，以将3D点云过滤为2D切割。

注意
在cartographer配置文件中，距离以米为单位定义

距离是在机器人实际移动过程中的一定时间内测量的。 但是，在大量的ROS message 中距离是“整批”传递的。 cartographer可以独立考虑每个message的时间戳，可以消减机器人运动引起的变形。 cartographer获得测量的次数越多，使测量不变形以组装可能立即捕获的单个相干扫描的效果就越好。 因此，强烈建议通过扫描（可以与另一扫描匹配的一组距离数据）提供尽可能多的距离数据（ROS消息）。

TRAJECTORY_BUILDER_nD.num_accumulated_range_data

距离数据通常是从机器人上的单个点以多个角度测量的。 这意味着经常会碰到封闭的表面（例如道路）并提供很多点。 相反，远处的物体受到撞击的次数较少，提供的分数也较少。 为了减少点处理的计算权重，我们通常需要对点云进行子采样。 但是，简单的随机采样会从我们已经具有低测量密度的区域中删除点，而高密度区域仍将具有比所需更多的点。 为了解决该密度问题，我们可以使用体素滤波器，将原始点降采样为恒定大小的多维数据集，并且仅保留每个多维数据集的质心。

较小的多维数据集大小将导致更密集的数据表示，从而导致更多的计算。 大的多维数据集大小将导致数据丢失，但速度会更快。

TRAJECTORY_BUILDER_nD.voxel_filter_size

应用固定大小的voxel filter(体素滤波器)后，cartographer还应用了adaptive voxel filter(自适应体素滤器)。该过滤器尝试确定最佳体素大小（最大长度以下）以达到目标点数。 在3D中，两个自适应体素滤镜用于生成高分辨率和低分辨率点云，在local SLAM中将阐明它们的用法。

TRAJECTORY_BUILDER_nD.*adaptive_voxel_filter.max_length
TRAJECTORY_BUILDER_nD.*adaptive_voxel_filter.min_num_points

惯性测量单元(IMU)可以成为SLAM的有用信息来源，因为它可以提供准确的重力方向（因此可以找到水平面）以及带有噪声但完整的机器人旋转指示。 为了滤除IMU噪声，需要在一定时间内观察重力。 如果您使用2D SLAM，则可以实时处理范围数据而无需其他信息源，因此您可以选择是否要让制图师使用IMU。 使用3D SLAM，您需要提供一个IMU，因为它被用作扫描方向的初始猜测，从而大大降低了扫描匹配的复杂性。

TRAJECTORY_BUILDER_2D.use_imu_data
TRAJECTORY_BUILDER_nD.imu_gravity_time_constant

注意 在cartographer配置文件中，每个时间值以秒为单位定义

Local SLAM

一旦扫描组合完毕并从多个范围数据中过滤掉后，就可以使用local SLAM算法了。 local SLAM通过使用姿势外推器计算的值作为新scan姿态的初值进行扫描匹配，将新scan插入其当前submap结构中。 姿势外推器的思想是使用测距仪之外的其他传感器的数据来预测下一次scan应插入到submap中的位置。

有两种扫描匹配策略：

• CeresScanMatcher将姿态外推器的初始值作为scan位姿的先验值，使scan匹配到适合子图的最佳位置。 它通过内插子···········································································································································································································································································································································································································································································1图和对齐扫描的子像素来实现。 这是快速的，但无法修复明显大于子图分辨率的错误。 如果您的传感器设置和计时合理，通常最好使用CeresScanMatcher
• 如果您没有其他传感器或您不信任它们，则可以启用RealTimeCorrelativeScanMatcher。 它使用一种类似于在闭环中将扫描与子图进行匹配的方法（稍后描述），而是与当前子图进行匹配。 然后，将最佳匹配用作CeresScanMatcher的先前匹配。 该扫描匹配器非常昂贵，并且将覆盖来自测距仪的其他传感器的任何信号，但是在功能丰富的环境中具有强大的鲁棒性。

无论哪种方式，都可以将CeresScanMatcher配置为对其每个输入赋予一定的权重。 权重是对数据信任度的度量，可以看作是静态协方差。 重量单位参数是无量纲的数量，不能相互比较。 数据源的权重越大，cartographer在进行扫描匹配时将更加重视此数据源。 数据源包括占用的空间（来自扫描的点），姿势推断器（或RealTimeCorrelativeScanMatcher）的平移和旋转

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

注意: 在3D中，被occupied_space_weight_0和occupied_space_weight_1参数分别与高分辨率和低分辨率滤波点云相关。

CeresScanMatcher的名称来自谷歌开发的用于解决非线性最小二乘问题的图书馆Ceres Solver。 扫描匹配问题被建模为这种问题的最小化，其中两次扫描之间的运动（变换矩阵）是要确定的参数。 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。 它通过在由最大距离半径和最大角度半径定义的搜索窗口中搜索相似的扫描来工作。 使用此窗口中的扫描进行扫描匹配时，可以为平移和旋转分量选择不同的权重。 例如，如果您知道自己的机器人旋转不多，则可以使用这些权重。

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

为避免每个子图插入太多扫描，一旦扫描匹配器发现两次扫描之间有运动，它将通过运动过滤器。 如果导致其移动的运动不够重要，则将放弃扫描。 仅当扫描的运动超过一定距离，角度或时间阈值时，扫描才会插入到当前子图中。

TRAJECTORY_BUILDER_nD.motion_filter.max_time_seconds
TRAJECTORY_BUILDER_nD.motion_filter.max_distance_meters
TRAJECTORY_BUILDER_nD.motion_filter.max_angle_radians

当本地SLAM已接收到给定数量的范围数据时，则认为子图已完成。 本地SLAM随时间漂移，使用全局SLAM修复此漂移。 子图必须足够小，以使其内部的漂移低于分辨率，以便局部正确。 另一方面，它们应该足够大以使环路闭合正常工作。

TRAJECTORY_BUILDER_nD.submaps.num_range_data

submap可以将距离数据存储在两个不同的数据结构中：最广泛使用的表示形式称为概率网格。 但是，在2D模式下，也可以选择使用“截断符号距离字段（TSDF）”。

TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.grid_type

概率网格将空间切成2D或3D表，其中每个像元的大小固定，并包含被遮挡的几率。 根据“命中”（测量范围数据的位置）和“未命中”（传感器与测量点之间的自由空间）更新赔率。 命中率和未命中率在占用概率计算中可以具有不同的权重，从而或多或少地信任占用或空闲空间的测量结果。

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中，出于扫描匹配性能的原因，使用了两个混合概率网格。（术语“杂合”仅指一个内部树状数据表示和被抽象给用户）

a low resolution hybrid grid for far measurements
a high resolution hybrid grid for close measurements

扫描匹配通过将低分辨率点云的远点与低分辨率混合网格对齐来开始，然后通过将近距离高分辨率点与高分辨率混合网格对齐来优化姿态。

TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution
TRAJECTORY_BUILDER_3D.submaps.high_resolution
TRAJECTORY_BUILDER_3D.submaps.low_resolution
TRAJECTORY_BUILDER_3D.high_resolution_adaptive_voxel_filter.max_range
TRAJECTORY_BUILDER_3D.low_resolution_adaptive_voxel_filter.max_range

Note:
制图师ROS提供了一个RViz插件来可视化子图。 您可以从编号中选择要查看的子图。 在3D中，RViz仅显示3D混合概率网格的2D投影（灰度）。 RViz的左窗格中提供了选项，可在低分辨率和高分辨率混合网格可视化之间切换。

**TODO：**记录TSDF配置

Global SLAM

当本地SLAM生成其连续的子图时，全局优化（通常称为“优化问题”或“稀疏姿势调整”）任务在后台运行。 它的作用是在彼此之间重新排列子图，以便它们形成一个连贯的全局图。 例如，此优化负责更改当前构建的轨迹，以根据循环闭合正确地对齐子图。

一旦插入一定数量的轨迹节点，优化将分批运行。 根据需要运行的频率，可以调整这些批处理的大小。

POSE_GRAPH.optimize_every_n_nodes

注意：
将POSE_GRAPH.optimize_every_n_nodes设置为0是禁用global SLAM并专注于local SLAM行为的简便方法。
这通常是调整cartographer的第一件事。

global SLAM是“ GraphSLAM”的一种，本质上是一种姿态图优化，其工作原理是在nodes 和submaps 之间建立constraints ，然后对所得约束图进行优化。 可以直观地将约束视为将所有节点捆绑在一起的细绳。 稀疏的姿势调整将所有绳索完全固定。 所得的网络称为“pose graph”。

注意
约束可以在RViz中可视化，调整global SLAM非常方便。还可以切换POSE_GRAPH.constraint_builder.log_matches获取格式化为直方图的约束生成器的常规报告。

• 非全局约束(也称为内部子映射约束)是在节点之间自动构建的，这些节点在轨迹上紧密地跟随彼此。直观地说，这些非全局绳索保持了轨迹的局部结构一致。
• 全局约束(也称为循环闭合约束或内部子映射约束)通常在新子映射和之前的节点之间进行搜索，这些节点在空间上足够接近(某个搜索窗口的一部分)，并且具有很强的匹配性(运行扫描匹配时的良好匹配)。直观地说，这些全局绳索在结构中引入了节点，并牢固地将两股绳系在一起。

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只考虑用于构建约束的所有关闭节点的子采样集。这是由采样比常数控制的。采样过少的节点可能会导致遗漏约束和无效的循环闭包。采样过多的节点会减慢全局关闭的速度，并防止实时的循环关闭

POSE_GRAPH.constraint_builder.sampling_ratio

当考虑节点和子映射以进行约束构建时，它们将首先经过一个称为FastCorrelativeScanMatcher的扫描匹配器。该扫描匹配器是专为cartographer设计的，使 real-time loop closures scan matching成为可能。FastCorrelativeScanMatcher依赖于一个 “分支定界” 机制来在不同的网格分辨率下工作，并有效地消除不正确的匹配。这种机制在本文前面介绍的cartographer论文中得到了广泛的介绍。它在可控制深度的探测树上工作。

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运行 optimization problem时，Ceres用于根据多个残差重新排列submaps。残差采用加权成本函数计算。global optimization具有考虑大量数据源的成本函数:全局(闭环)约束、非全局(匹配)约束、IMU加速度和旋转测量、局部SLAM粗略位姿估计、里程测量源或固定帧(如GPS系统)。权值和Ceres选项可以按照local SLAM部分的描述进行配置。

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

注意
通过toggling（绷紧） pose_graph .max_num_final_iteration可以找到关于优化问题中使用的残差的有用信息

作为IMU残差的一部分， optimization problem 为IMU姿态提供了一定的灵活性，在默认情况下，Ceres可以自由地优化IMU和跟踪框架之间的外部校准。如果你不相信你的IMU姿态，Ceres的全局优化结果可以被记录下来，用来改进你的外部校准。如果Ceres没有正确地优化你的IMU姿势，而你足够相信你的外部校准，你可以使这个姿势不变。

POSE_GRAPH.optimization_problem.log_solver_summary
POSE_GRAPH.optimization_problem.use_online_imu_extrinsics_in_3d

在残差中，异常值的影响由配置了一定Huber scale的Huber loss函数来处理。Huber scale越大，(潜在的)异常值的影响就越大。

POSE_GRAPH.optimization_problem.huber_scale

一旦 trajectory完成，cartographer就会运行一个新的global optimization，通常会比以前的global optimization进行更多的迭代。这样做是为了优化cartographer的最终结果，通常不需要是实时的，因此大量的迭代通常是正确的选择。

POSE_GRAPH.max_num_final_iterations