Local SLAM的工作是建立一系列的子图。每一个子图是局部一致的，但是可以接受它随着时间会发生漂移。

本文件中，距离的单位： 米， 角度的单位： 度，时间的单位： 秒

基本

use_imu_data = true,
min_range = 0.,
max_range = 30.,
# 设置雷达数据在z轴上的滤波，只用于多线雷达，单线雷达无需使用
min_z = -0.8,
max_z = 2.,

missing_data_ray_length = 5.,
num_accumulated_range_data = 1,
# 使用voxel滤波时立方体边长的大小，这个值是固定的
voxel_filter_size = 0.025,

• use_imu_data： 进行2D SLAM时，Cartographer的默认配置是使用imu的，如果不想要imu就要TRAJECTORY_BUILDER_2D.use_imu_data = false。在源码LocalTrajectoryBuilder2D::AddRangeData中可以看到，如果设置为false，会直接初始化位姿估计器；如果设置为true，会等收到imu数据后初始化位姿估计器，如果没有启动imu程序，Cartographer会一直等待imu的数据而无法进行。

如果不用imu的话，构图的时候一定要移动的很慢。3D SLAM必须使用imu，因为它提供了重力的精确方向和激光方向的一个初始猜测值，巨大地减少了scan matching的复杂度。 代码在Node::ComputeExpectedSensorIds

// 2D的IMU可选，3D的IMU必须有
if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
      (node_options_.map_builder_options.use_trajectory_builder_2d() &&
       options.trajectory_builder_options.trajectory_builder_2d_options()
           .use_imu_data())) 
{
  expected_topics.insert(SensorId{SensorType::IMU, topics.imu_topic});
}

• 从min_range到max_z： 相当于带通滤波，减少传感器的噪声，z用于3D SLAM，在源码的sensor::CropRangeData函数。min_range和max_range用于局部地图下的激光点测量范围。

如果对着空旷的环境建图，会运行到AddAccumulatedRangeData的Dropped empty horizontal range data然后返回，不会有地图

• missing_data_ray_length: 官方的解释是 Points beyond ‘max_range’ will be inserted with this length as empty space.这是用于处理miss集合的，但是我看了源码之后，有所疑惑。源码部分：

const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;
const float range = delta.norm();

用range跟min_range 和 max_range比较，在范围内的插入 accumulated_range_data_.returns，超出 max_range的做如下处理
hit_in_local.position = origin_in_local + options_.missing_data_ray_length() / range * delta

也就是相当于把hit_in_local = origin + delta变成了hit_in_local = origin + missing_data_ray_length/delta.norm() * delta，delta多了个因子。没看明白为什么这样计算，暂时让missing_data_ray_length小于max_range，这个参数显然跟雷达相关。

• num_accumulated_range_data： 几帧有效的点云才进行一次扫描匹配。 从现象上看这和减少运动带来的雷达数据的畸变有关。Cartographer把num_accumulated_range_data个帧集合 (accumulate) 成一个大帧作为算法的输入。
  Cartographer认为这每一帧都是独立的，以帧为单位补偿运动导致的激光雷达数据产生的畸变，然后再把这些帧集合到一块去。所以Cartographer接收到的数据帧的频率越高，Cartographer 的补偿效果越好，算法的输入数据质量越高。
  当我设置为10后，雷达的scan更新很慢，跟不上机器人的运动，所以对2D雷达一般都设置为1。如果同时用两个雷达，每个的一帧激光用1个ROS消息，那么设置为2。 在源码中的LocalTrajectoryBuilder2D::AddRangeData
  

• voxel_filter_size: 离雷达较近的表面 (如路面) 经常得到更多采样点, 而远处的物体的采样点比较稀少。为了降低计算量，需要对点云数据进行体素滤波，简单的随机采样仍然会导致低密度区的点更少，而高密度区的点仍然比较多。voxel_filter_size即为立方体的大小。如果立方体较小的话会导致密集的数据，所耗的计算量更大。较大的话可能会导致数据丢失，但是计算速度会更快。一般就用0.025，最大不要超过0.1

自适应的voxel滤波

使用固定的voxel滤波之后，再使用自适应的voxel滤波，voxel_filter用于生成稀疏点云，以进行扫描匹配

adaptive_voxel_filter = { 
	max_length = 0.5,       # 尝试确定最佳的立方体边长，边长最大为0.5
	# 如果存在较多点，并且大于min_num_points，则减小体素长度以尝试获得该最小点数
  # 如果point_cloud.size()小于该参数则返回，说明点云已经够稀疏了
	min_num_points = 200,
  # 这个是相对local坐标系原点的
	max_range = 50.,        # 距远离原点超过max_range 的点被移除 
},
# 用于生成稀疏点云 以进行 闭环检测
loop_closure_adaptive_voxel_filter = {
	max_length = 0.9,       
	min_num_points = 100,
	max_range = 50.,
},

在提供了定大小的 voxel_filter, Cartographer 还提供了一个adaptive_voxel_filter, 可以在最大边长max_length的限制下优化确定voxel_filter_size来实现目标的points数min_num_points。 如果未进入二分法过滤，点云数量会比min_num_points稍微大一点； 如果进入了二分法过滤， min_num_points <= 最终点云数 <= min_num_points x 1.1

在3D中，使用两个自适应的体素滤波器来生成一个更高分辨率和一个低分辨率的点云，其使用方法在Local SLAM.

adaptive_voxel_filter的参数不是没用，我调整max_length=3之后，地图中出现了这样的情况：

改成2就好了，默认的1也行

pose_extrapolator

• imu_based = false     3d用来初始化位姿推断器的方式。相关参数全省略

costant_velocity = {
  imu_gravity_time_constant = 10
  pose_queue_duration = 0.001
}

• imu_gravity_time_constant: 移动时通过imu观测10s，以确定重力的平均方向。参数为align重力的时间间隔，也是IMU一阶低通滤波中的滤波器常数，调用在ImuTracker::AddImuLinearAccelerationObservation，平时不必修改，如果要认真修改，最好使用Matlab理解一阶滤波算法

• pose_queue_duration

两个scan matcher

real_time_correlative_scan_matcher

use_online_correlative_scan_matching = false,
real_time_correlative_scan_matcher = 
{
  #  一般情况下，位姿估计器传来的初值与scan matcher找到的初值不会差很多
  #  所以搜索窗口不必很大，0.5已经比较大了
  # 找到最佳扫描匹配的  最小线性搜索窗口，0.1就是2个栅格
  linear_search_window = 0.1,
  # 找到最佳扫描匹配的  最小角度搜索窗口，默认20°是比较大的
  angular_search_window = math.rad(20.),

  # 用于计算各部分score的权重，如果一个平台的瞬间平移很少，
  # 则可以降低平移的权重，旋转也是一样
  # 源码在 ScoreCandidates
  translation_delta_cost_weight = 1e-1,
  rotation_delta_cost_weight = 1e-1,
},

use_online_correlative_scan_matching默认 true。如果这项为false，则直接用 ceres_scan_matcher

使用类似实时的闭环检测的方法进行前端的扫描匹配，将当前scan在一定的搜索范围内搜索，范围为设定的平移距离及角度大小，然后在将scan插入到匹配的最优位置处。设置为true后，建图的效果非常好，即使建图有漂移也能够修正回去，但是计算复杂度非常高，很耗CPU。 在低配置平台上，一般不用

ceres_scan_matcher

# 将前一时刻的位姿作为先验，使用odom或者imu的数据提供初值，以确定scan的最优位姿
ceres_scan_matcher = 
{
  occupied_space_weight = 1.,   # 每个Cost因素的 尺度因子
  translation_weight = 10., 
  rotation_weight = 40.,    
  ceres_solver_options = {
    use_nonmonotonic_steps = false,
    max_num_iterations = 20,
    num_threads = 1,
  },
},

CeresScanMatcher可以针对它的输入设置权重，体现了对数据的信任程度。某种数据的权重越大，Cartographer在做scan matching时就更重视这种数据。数据有：occupied space(来自scan中的点), 来自pose extrapolator(或者RealTimeCorrelativeScanMatcher)的平移和旋转

计算位姿估计器提供的预测值和ceres scan matcher返回的位姿之间的差

double variety_distance = (pose_estimate_2d->translation() - pose_prediction.translation()).norm();

结果发现variety_distance在毫米级，甚至更小，可见位姿估计器提供的预测值、实时scan matcher输出的初值、 ceres scan matcher输出的位姿三者差距都很小，这里的参数不必做太多调整。 CeresScanMatcher的CPU消耗主要在 OccupiedSpaceCostFunction ，即使在源码里将位移和旋转部分去掉，无论精度还是CPU，影响都不明显。

试验发现前端不经过两个scan matcher，所建的图略有偏差，但尚可接受。推测如果IMU和里程计精度足够，对建图精确性要求不高，前端不用两个scan matcher也行，但是占用CPU下降不多，可见优化的重点还在后端

• max_num_iterations: 在ceres中默认是50，这里改为20是因为SLAM的前端有实时性要求，所以不能太大。可以根据自己的情况继续降低, ALOAM中甚至设置为 4。
  另外源码中使用的求解方式是DENSE_QR，因为如果ceres优化问题不是SLAM的大型后端，不是稀疏问题，可使用DENSE_QR

motion_filter

# 只有当scan的平移、旋转或者时间 超过阈值时，才会被加入到 submap 中，不超过则舍弃
  motion_filter = {
    max_time_seconds = 5.,
    max_distance_meters = 0.2,
    max_angle_radians = math.rad(1.),
  },

为了避免每个子图插入太多的scan，一旦scan matcher发现两个scan之间有motion，它就会进入motion filter. 只要scan的motion达到一个参数要求，就会插入到当前子图，否则被抛弃。 建图时会发现，如果机器人停着不动，过一段时间图会逐渐形成轮廓，边界变黑变粗，这就是max_time_seconds决定的，可以稍微降低。

代码在LocalTrajectoryBuilder2D::InsertIntoSubmap的开头，只调用一个函数MotionFilter::IsSimilar，常见的日志是Motion filter reduced the number of nodes to 3.8%，可见过滤效果很强

submaps

# 平时可能修改的只有 num_range_data
submaps = 
{
  # 每个submap的scan总数为num_range_data的2倍，前一半进行初始化而不匹配，后一半进行匹配
  num_range_data = 90,
  grid_options_2d = {
    grid_type = "PROBABILITY_GRID",
    resolution = 0.05,   # 栅格的尺寸
  },
  range_data_inserter = 
  {
    range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D",
    probability_grid_range_data_inserter = {
      insert_free_space = true,
      hit_probability = 0.55,   # 占用,  >= 0.5
      miss_probability = 0.49,  # 空闲,  <= 0.5
    },
  },

},

• num_range_data：子图由多少scan构成。这个参数对建图效果有很大影响，可以试试50~90。当Local SLAM收到足够的雷达数据后，子图才算完整

子图必须足够小，使其内部的漂移低于分辨率，以便它们是局部正确的。另一方面，它们应该足够大以使loop closure能够正常工作。 对应的源码在ActiveSubmaps2D::InsertRangeData

这个值与CPU有这样一种关系：值比较小(10)，CPU使用率比较稳定，但整体偏高；值大时，CPU平时使用率低，但会短暂爆增。如果再增大(140)，CPU长时间占用低，偶尔才出现一两次高峰。所以可以认为 num_range_data越小，CPU占用的高峰越多，可能就是插入子图完成的时候增大CPU占用，估计是函数 ApplyLookUpTable 导致

• grid_type：submap存储雷达数据的格式，最常用的是概率栅格。在2D中，另一种是Truncated Signed Distance Fields (TSDF). 源码在ActiveSubmaps2D::CreateGrid

• resolution: 栅格尺寸，常常是0.05。并不是越小越好，我在2000米的环境下设置为0.02，结果建图严重错位，全局优化也没有纠正过来，应该是算力不够了。设置为0.05，对后端参数不必特别设置，建图效果也很好。

• range_data_inserter： 概率栅格把空间剪切成一个2D或者3D的表格，该表格的每一个cell有一个固定的大小，并包含了被构造(obstructed)的可能性。可能性是根据hits(测量范围数据)和misses(传感器和测量点之间的自由空间)来进行更新，hits和misses二者在占据概率计算中可能有不同的权重，该占据概率计算或多或少相信占据或者自由空间的测量。

• insert_free_space对应的源码在CastRays函数，是否处理miss事件，如果为 false，则free的栅格不会改变占用网格中的概率。 后两个参数在ProbabilityGridRangeDataInserter2D的构造函数中使用。

概率栅格地图的Odds are updated according to “hits” (where the range data is measured) and “misses” (the free space between the sensor and the measured points)。可以根据对被占据 occupied 和 free space 的雷达数据的置信度，加减 hits 和 misses 的 weight 值（Both hits and misses can have a different weight in occupancy probability calculations giving more or less trust to occupied or free space measurements）。

在2D中，每个子图只有一个概率栅格地图被存储。在3D中，由于scan matching性能的原因，两个混合的概率栅格被使用，并分别应用了一个adaptive_voxel_filter。对于远距离测量，使用一个低分辨率的混合栅格；对于近距离测量，使用一个高分辨率的混合栅格；scan matching先使用低分辨率的点云来与低分辨率的栅格进行对齐，然后通过高分辨率的近处点云与高分辨率的 hybrid grid 对齐来 refine pose。

RViz插件可以观察子图，可以根据索引选择子图