cartographer探秘第一章之参数配置
第一步 安装与编译
具体的安装步骤参考官方文档即可，链接如下。值得注意的是，一定要确保每个步骤都安装成功，尤其是Ninja。
https://google-cartographer-ros.readthedocs.io/en/latest/compilation.html
其中ceres1.13.0是需要才能下载的，可以直接下载我上传的代码，cartographer的2019年5月28日的安装代码(release-1.0)，包含了proto,cartographer,cartographer_ros以及ceres1.13.0版本，要从proto编译那步进行编译，也就是
src/cartographer/scripts/install_proto3.sh 这步，只不过我这个包已经下载好了proto，可以直接删掉重新下载，也可以自行按照这个脚本编译一下。
...很奇怪，竟然不能自己选择下载需要的积分，没想收费的。
第二步 参数配置
安装好了之后试着跑一下demo，官方文档中的那几个demo包的下载需要，下好了直接就能跑，只要把bag包放在Download文件夹下即可。
如果想要使用自己的机器人跑carto该怎么去修改参数配置文档呢，也就是lua文件。
carto的默认参数配置在cartographer/configuration_files文件夹下，其中有2个起总领作用，分别是map_builder.lua以及trajectory_builder.lua。两个文件的内容如下：
# map_builder.lua
include "pose_graph.lua"
MAP_BUILDER = {
use_trajectory_builder_2d = false,
use_trajectory_builder_3d = false,
num_background_threads = 4,
pose_graph = POSE_GRAPH,
collate_by_trajectory = false,
}
# trajectory_builder.lua
include "trajectory_builder_2d.lua"
include "trajectory_builder_3d.lua"
TRAJECTORY_BUILDER = {
trajectory_builder_2d = TRAJECTORY_BUILDER_2D,
trajectory_builder_3d = TRAJECTORY_BUILDER_3D,
-- pure_localization_trimmer = {
-- max_submaps_to_keep = 3,
-- },
collate_fixed_frame = true,
collate_landmarks = false,
}可以看到，map_build.lua配置了是使用trajectory_build_2d还是trajectory_build_3d，以及后端使用的线程数。trajectory_build就是前端具体的参数配置，分为2d和3d两个文件，而pose_graph.lua为后端优化的具体参数配置。那想要配置自己的硬件实现carto该怎么做呢，接下来我照着carto的demo的配置文件backpack_2d.lua进行讲解一下。
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "base_link", # 进行slam的坐标系，如果有imu的话为imu的坐标系，没有的话就是base_link 或者 base_footprint
published_frame = "base_link", # carto发布的坐标系
odom_frame = "odom", # 里程计数据的坐标系名称
provide_odom_frame = true, # 是否发布里程计坐标系，适用于机器人本身没有odom坐标系的情况
publish_frame_projected_to_2d = false,
use_pose_extrapolator = true,
use_odometry = false, # 是否使用odom数据
use_nav_sat = false, # 是否使用gps数据
use_landmarks = false, # 是否使用landmarks
num_laser_scans = 0, # 如果用的是单线雷达，此处为雷达的数量
num_multi_echo_laser_scans = 1,
num_subdivisions_per_laser_scan = 10, # 将雷达一帧的数据拆分成几次发出来，对于普通雷达的驱动包来说，此处应为1
num_point_clouds = 0, # 多线雷达的数量
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
trajectory_publish_period_sec = 30e-3,
rangefinder_sampling_ratio = 1., # 接下来的5个参数为5种观测的权重比
odometry_sampling_ratio = 1., # 如odom的数据非常不准，可以设置为0.3以减小odom对整体优化的影响
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}
MAP_BUILDER.use_trajectory_builder_2d = true # 具体是使用2d 还是 3d 的slam
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 10 # 几个雷达，以及一帧数据被拆成几次发出来，对于1个激光雷达，普通的雷达驱动包来说，此处应为1
return options这是对默认的配置文件进行了简单的分析，但是该怎样配置自己的硬件的lua呢？
# myrobot_2d.lua
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "base_link",
published_frame = "base_link",
odom_frame = "odom",
provide_odom_frame = true,
publish_frame_projected_to_2d = false,
use_pose_extrapolator = true,
use_odometry = false,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 1,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
trajectory_publish_period_sec = 30e-3,
rangefinder_sampling_ratio = 1.,
odometry_sampling_ratio = 1.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}
MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.min_range = 0.
TRAJECTORY_BUILDER_2D.max_range = 30.
return options以上配置了使用1个单线激光雷达进行2d slam 的lua文件，并且是在机器人本身没有提供odom坐标系的情况下。以上配置出来的tf为 map->odom->base_link。
这几个坐标系的名字都是干嘛的呢？
map_frame始终为map，这个不用动。tracking_frame也可以始终为base_link不用动。provide_odom_frame 为 false时，carto发布的坐标变化为 map_frame -> published_frame ，也就是map到base_link，如果provide_odom_frame为 true时，carto就会发布map -> odom -> base_link 的坐标变化。
所以，如果机器人本身存在odom坐标系，而你又想让carto 使用 odom数据 ，就需要将published_frame 设置为odom，provide_odom_frame = false 而use_odometry = true。
之后MAP_BUILDER.use_trajectory_builder_2d为使用2d slam 。在进行2d slam 时，carto的默认配置是使用imu的，所以如果没有imu就要TRAJECTORY_BUILDER_2D.use_imu_data = false，否则会一直等待imu的数据而进行不下去。而3d 的 slam 必须使用imu，所以就没有这个参数配置。
再之后 TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true，这一项配置很重要，对于我自己使用的雷达或者在 gazebo 中仿真时，如果不配置这项得到的建图效果将非常差。但是carto得demo没有进行这一项的配置也跑的很好，这让我很费解，难道是因为我没使用imu的问题？？？
那这个参数是干嘛的呢？
这个参数配置的是否使用 实时的闭环检测方法 来进行前端的扫描匹配。如果这项为false，则扫描匹配使用的是通过前一帧位置的先验，将当前scan与之前做对比，使用 高斯牛顿法 迭代 求解最小二乘问题 求得当前scan的坐标变换。如果这项为true，则使用闭环检测的方法，将当前scan在一定的搜索范围内搜索，范围为设定的平移距离及角度大小，然后在将scan插入到匹配的最优位置处。这种方式建图的效果非常好，即使建图有漂移也能够修正回去，但是这个方法的计算复杂度非常高，非常耗cpu。
到这里2d 的 slam 应该能够使用了。在carto demo 的 launch 中配置了 urdf 模型，如果机器人的urdf 模型已经单独启动了，那么就不需要在carto的launch中配置urdf了。carto的launch 和 lua 配置文件都是在install_isolated/share/cartographer_ros/launch/ 和 install_isolated/share/cartographer_ros/configuration_files/ 下，所以如果只修改了cartographer_ros软件包中的launch 和 lua 文件之后是需要重新编译的，编译之后才能将修改的文件安装到install_isolated文件中。编译的命令为：
source install_isolated/setup.bash
catkin_make_isolated --install --use-ninja第三步 参数说明
3.1 trajectory_builder_2d.lua
TRAJECTORY_BUILDER_2D = { # 距离的单位： 米， 角度的单位： 度，时间的单位： 秒
use_imu_data = true,
min_range = 0., # 雷达数据的最远最近滤波
max_range = 30.,
min_z = -0.8, # 还可以设置雷达数据在z轴上的滤波将3d数据转成2d的
max_z = 2.,
missing_data_ray_length = 5., # 超出'max_range'的点将以此长度作为free空间插入
num_accumulated_range_data = 1, # carto建议将一帧雷达数据分成几个ros的消息发，以减少运动带来的雷达数据的畸变
voxel_filter_size = 0.025, # 使用voxel滤波时立方体边长的大小，这个值是固定的
# 使用固定的voxel滤波之后，再尝试使用自适应的voxel滤波，体素滤波器 用于生成稀疏点云 以进行 扫描匹配
adaptive_voxel_filter = {
max_length = 0.5, # 尝试确定最佳的立方体边长，边长最大为0.5
min_num_points = 200, # 如果存在 较多点 并且大于'min_num_points'，则减小体素长度以尝试获得该最小点数
max_range = 50., # 距远离原点超过max_range 的点被移除
},
loop_closure_adaptive_voxel_filter = {
max_length = 0.9,
min_num_points = 100, # 体素滤波器 用于生成稀疏点云 以进行 闭环检测
max_range = 50.,
},
# 是否使用 real_time_correlative_scan_matcher 解决 在线的扫描匹配 问题，从而为ceres提供先验信息
use_online_correlative_scan_matching = false,
# 有2种解决 扫描匹配 问题 的方法
# 通过 类似闭环检测 的方式找到最优匹配，然后作为 先验，再使用ceres_scan_matcher 进行求解
# 计算复杂度高，但是很鲁棒，在odom或者imu不准时依然能达到很好的效果
# 如果一个平台的瞬间平移很少，则可以降低平移的权重，旋转也是一样
real_time_correlative_scan_matcher = {
linear_search_window = 0.1, # 找到最佳扫描匹配的最小线性搜索窗口
angular_search_window = math.rad(20.), # 找到最佳扫描匹配的最小角度搜索窗口
translation_delta_cost_weight = 1e-1, # 用于计算各部分score的权重
rotation_delta_cost_weight = 1e-1,
},
# 将前一时刻的位姿作为先验，使用odom或者imu的数据进行预测，以确定scan的最优位姿
ceres_scan_matcher = {
occupied_space_weight = 1., # 每个Cost因素的 尺度因子
translation_weight = 10.,
rotation_weight = 40.,
ceres_solver_options = { # see https://code.google.com/p/ceres-solver/
use_nonmonotonic_steps = false,
max_num_iterations = 20,
num_threads = 1,
},
},
# 只有当scan的 平移、旋转或者时间 超过阈值时 才会被加入到 submap 中，不超过则舍弃
motion_filter = {
max_time_seconds = 5.,
max_distance_meters = 0.2,
max_angle_radians = math.rad(1.),
},
imu_gravity_time_constant = 10., # 移动时通过imu观测10s以确定重力的平均方向
submaps = {
num_range_data = 90, # 每个submap的scan总数为num_range_data 的2倍，前一半进行初始化而不进行匹配，后一半进行匹配
grid_options_2d = {
grid_type = "PROBABILITY_GRID", # submap 存储雷达数据的格式，还可以为Truncated Signed Distance Fields (TSDF)
resolution = 0.05, # 0.05米 栅格的尺寸
},
range_data_inserter = {
range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D",
probability_grid_range_data_inserter = {
insert_free_space = true, # 如果为 false，则free的栅格不会改变 占用网格 中的概率。
hit_probability = 0.55, # hit(占用) 时的概率
miss_probability = 0.49, # miss（空闲） 时的概率
},
tsdf_range_data_inserter = {
truncation_distance = 0.3,
maximum_weight = 10.,
update_free_space = false,
normal_estimation_options = {
num_normal_samples = 4,
sample_radius = 0.5,
},
project_sdf_distance_to_scan_normal = true,
update_weight_range_exponent = 0,
update_weight_angle_scan_normal_to_ray_kernel_bandwidth = 0.5,
update_weight_distance_cell_to_hit_kernel_bandwidth = 0.5,
},
},
},
}3.2 trajectory_builder_3d.lua
MAX_3D_RANGE = 60.
TRAJECTORY_BUILDER_3D = {
min_range = 1.,
max_range = MAX_3D_RANGE,
num_accumulated_range_data = 1,
voxel_filter_size = 0.15,
# 在3d slam 时会有2个自适应滤波，一个生成高分辨率点云，一个生成低分辨率点云
high_resolution_adaptive_voxel_filter = {
max_length = 2.,
min_num_points = 150,
max_range = 15.,
},
low_resolution_adaptive_voxel_filter = {
max_length = 4.,
min_num_points = 200,
max_range = MAX_3D_RANGE,
},
use_online_correlative_scan_matching = false,
real_time_correlative_scan_matcher = {
linear_search_window = 0.15,
angular_search_window = math.rad(1.),
translation_delta_cost_weight = 1e-1,
rotation_delta_cost_weight = 1e-1,
},
# 在3D中，occupied_space_weight_0和occupied_space_weight_1参数分别与高分辨率和低分辨率滤波点云相关
ceres_scan_matcher = {
occupied_space_weight_0 = 1.,
occupied_space_weight_1 = 6.,
translation_weight = 5.,
rotation_weight = 4e2,
only_optimize_yaw = false,
ceres_solver_options = {
use_nonmonotonic_steps = false,
max_num_iterations = 12,
num_threads = 1,
},
},
motion_filter = {
max_time_seconds = 0.5,
max_distance_meters = 0.1,
max_angle_radians = 0.004,
},
imu_gravity_time_constant = 10.,
rotational_histogram_size = 120, # rotational scan matcher 的 histogram buckets
# 在3D中，出于扫描匹配性能的原因，使用两个混合概率网格。 （术语“hybrid”仅指内部树状数据表示并被抽象给用户）
submaps = {
high_resolution = 0.10, # 用于近距离测量的高分辨率hybrid网格 for local SLAM and loop closure，用来与小尺寸voxel进行精匹配
high_resolution_max_range = 20., # 在插入 high_resolution map 之前过滤点云的最大范围
low_resolution = 0.45, # 用于远距离测量的低分辨率hybrid网格 for local SLAM only，用来与大尺寸voxel进行粗匹配
num_range_data = 160,
range_data_inserter = {
hit_probability = 0.55,
miss_probability = 0.49,
num_free_space_voxels = 2, # 最多可更新多少个free space voxels以进行扫描匹配，0禁用free space
},
},
}3.3 pose_graph.lua
POSE_GRAPH = {
optimize_every_n_nodes = 90, # 每几个节点（节点是什么？scan?）执行一次优化，设为0即关闭后端优化
constraint_builder = {
sampling_ratio = 0.3, # 如果添加的约束与潜在约束的比例低于 此比例，则将添加约束。
max_constraint_distance = 15., # 在子图附近考虑姿势的阈值
min_score = 0.55, # 扫描匹配分数的阈值，低于该阈值时不考虑匹配。 低分表示扫描和地图看起来不相似
global_localization_min_score = 0.6, # 阈值，低于该阈值的全局定位不受信任
loop_closure_translation_weight = 1.1e4, # 优化问题中的 闭环约束 的 平移 的权重
loop_closure_rotation_weight = 1e5, # 优化问题中的 闭环约束 的 旋转 的权重
log_matches = true, # 闭环约束的直方图统计报告
fast_correlative_scan_matcher = { # 闭环检测的第一步
linear_search_window = 7., # 依靠“分支定界”机制在不同的网格分辨率下工作，并有效地消除不正确的匹配
angular_search_window = math.rad(30.), # 一旦找到足够好的分数（高于最低匹配分数），它（scan）就会被送入Ceres扫描匹配器以优化姿势。
branch_and_bound_depth = 7,
},
ceres_scan_matcher = { # 闭环检测的第二步 优化位姿图
occupied_space_weight = 20.,
translation_weight = 10.,
rotation_weight = 1.,
ceres_solver_options = {
use_nonmonotonic_steps = true,
max_num_iterations = 10,
num_threads = 1,
},
},
fast_correlative_scan_matcher_3d = {
branch_and_bound_depth = 8,
full_resolution_depth = 3,
min_rotational_score = 0.77,
min_low_resolution_score = 0.55,
linear_xy_search_window = 5.,
linear_z_search_window = 1.,
angular_search_window = math.rad(15.),
},
ceres_scan_matcher_3d = {
occupied_space_weight_0 = 5.,
occupied_space_weight_1 = 30.,
translation_weight = 10.,
rotation_weight = 1.,
only_optimize_yaw = false,
ceres_solver_options = {
use_nonmonotonic_steps = false,
max_num_iterations = 10,
num_threads = 1,
},
},
},
matcher_translation_weight = 5e2,
matcher_rotation_weight = 1.6e3,
optimization_problem = { # 残差方程的参数配置
huber_scale = 1e1, # Huber损失函数的尺度因子，该值越大，(potential) outliers（潜在）异常值 的影响越大。
acceleration_weight = 1e3, # IMU加速度的权重
rotation_weight = 3e5, # IMU旋转项的权重
local_slam_pose_translation_weight = 1e5, # 基于local SLAM的姿势在连续节点之间进行平移的权重
local_slam_pose_rotation_weight = 1e5, # 基于里程计的姿势在连续节点之间进行平移的权重
odometry_translation_weight = 1e5,
odometry_rotation_weight = 1e5,
fixed_frame_pose_translation_weight = 1e1, # FixedFramePose？应该是gps的
fixed_frame_pose_rotation_weight = 1e2,
log_solver_summary = false, # 可以记录Ceres全局优化的结果并用于改进您的外部校准
use_online_imu_extrinsics_in_3d = true, # 作为IMU残差的一部分
fix_z_in_3d = false,
ceres_solver_options = {
use_nonmonotonic_steps = false,
max_num_iterations = 50,
num_threads = 7,
},
},
max_num_final_iterations = 200, # 在建图结束之后会运行一个新的全局优化，不要求实时性，迭代次数多
global_sampling_ratio = 0.003,
log_residual_histograms = true,
global_constraint_search_after_n_seconds = 10.,
-- overlapping_submaps_trimmer_2d = {
-- fresh_submaps_count = 1,
-- min_covered_area = 2,
-- min_added_submaps_count = 5,
-- },
}
注: 目标计划
在3个月的时间中，我要完成carto的参数配置，最优参数的选择，carto的算法原理以及代码过程。并且将hokuyo, sick, 倍加福 3种单线激光雷达的建图效果对比出来，并分别配置出参数。以及 vlp-16 多线雷达的建图效果及参数。
所以这一章讲的是参数的意义，之后会将carto的论文及代码分析写出来。最后给出4种雷达的建图效果对比。接下来的第二章是对carto的论文进行分析，由于carto的前端使用的hector slam 的方法，所以还会顺带讲一下hector slam 的扫描匹配的最小二乘的公式推导。
由于本人正处于学习调研阶段，所以难免会有错误及疏漏，如果错误请指出，不胜感激！