Cartographer的后端参数设置和效果分析
任务动机：分析cartographer的后端相关的参数的效果和对性能的影响程度。
任务描述：按照官方文档google-cartographer-ros-readthedocs-io-en-latest.pdf第23页的说明（To reduce global SLAM latency部分），在cartographer默认参数的基础上，分别调整每个参数，保存地图和任务2.1要求的位姿序列，分析参数影响。
1. 测试方法
1. 1 测试包
使用官方的demo包b0-2014-08-14-13-23-01.bag 进行测试。使用以下命令行运行
roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=${HOME}/Downloads/b0-2014-08-14-13-23-01.bagdemo包参数信息
| 包名 | 预计运行时间 | 包大小 |
|---|---|---|
| b0-2014-08-14-13-23-01.bag | 768 s | 204 MB |
1.2 调整参数
根据官方文档google-cartographer-ros-readthedocs-io-en-latest.pdf在Chapter 5.3.1中提出的修改参数建议进行修改。
主要需要修改的文件在以下目录中
vim /cartographer/configuration_files/trajectory_builder_2d.lua 1.3 运行测试
基于张同学提供的自动记录py脚本提供的脚本进行安装和运行，使用以下命令行运行
rosrun （编译包名） csv.py在提示中：输入需要订阅的Topic为
Enter your ROS FIRST tf node ready to listen: /map
Enter your ROS SECOND tf node ready to listen: /base_link之后运行上述测试包，程序会自动纪录数据为tum数据格式和.xlsx格式。其中tum数据格式是一个.txt文件，其中记录了位移信息，四元数以及时间戳。
或者在script/csv.py中修改94-95行代码，更加方便。
（原本代码）
tf1 = raw_input("Enter your ROS FIRST tf node ready to listen:")
tf2 = raw_input("Enter your ROS SECOND tf node ready to listen:")（更改代码）
tf1 = '/map'
tf2 = '/base_link'注意
需要先运行该测试然后再运行demo包，不然会有信息缺失。
1.4 误差计算
1.4.1 使用 MATLAB 测量
基于邸老师提供的误差计算，制作了matlab脚本进行.xlsx文件的读取和计算。（参考仓库 Calculate Pose Error）
将csv.py输出文件作为该Script的输入端可以计算每个时刻的位移误差和旋转角度误差，可视化误差曲线和打印平均误差，作为cartographer的性能定量评测指标。
1.4.2 使用 evo 测量
基本信息
误差计算将基于输出的tum格式，使用evo进行分析和绘图。参考使用教程SLAM中evo的使用, 使用evo_traj 和 evo_rpe 进行计算和图像化。
安装信息
若没有安装pip，先安装pip。
运行以下命令行执行安装：
pip install evo --upgrade --no-binary evo 测试方法（在得到tum和.xlsx文件后）
绘制轨迹误差图像和位姿误差图像命令行
evo_traj tum [compareFileName] --ref=[controlFileName] -p --plot_mode=xy --align计算相对误差命令行
evo_rpe tum [compareFileName] [controlFileName] -p --plot_mode=xy --align如果出现以下错误
[ERROR] found no matching timestamps between ...计算出两个tum文件中的第一个时间戳的差值，在每个命令行后增加选项
--t_offset [差值]1.5 步骤小结
- 下载安装相关工具
- 运行python自动记录脚本
- 运行Demo包
- 在自动记录脚本中输入 /map 和 /base_link
- 使用evo计算和绘制
2. 效果分析
由于后端自带地图优化的系统，与前端测试十分不同的是，在绘制成型的地图上，地图误差非常的不明显。例如，下图是参数optimize_every_n_nodes几个参数的对比图，可以看到在直线区域（右下角）几条线基本重合。在整个绘制出来的地图中只有一两处，如下图的部分转弯角处，会产生一定相对明显的误差。
我们再来看这张图，这是xy轴上不同参数的对比，几乎所有的调参之后都会产生一定的图形位移，如下图。但整体的形状没有较大的改变，而且大多数图像都像这张图片一样，产生的位移十分小，在可接受范围内。
因此，鉴于各个参数的建图误差较小，在后端测试中，我们主要以evo_rpe测试出来的相对误差为衡量标准。
2.1 optimize_every_n_nodes
参数解释
大于零后会开启闭环优化，表示每n个点位进行优化
(官方文档 )
Online loop closure: If positive, will run the loop closure while the map is built.
默认值 = 90
类型: int32
测试结果（从右往左）
我们可以看到该参数在30左右平均误差达到最小值，且此时也处于最小方差位置，在30以上最大误差值都相对较小。在40-70左右平均误差相对稍高，在80时位置处在最高位，因此推荐参数修改范围为30-70。官方文档中建议减少该值来提高速度，因此推荐30左右配置较为良好。
2.2 MAP_BUILDER.num_background_threads
参数解释
计算线程数
(官方文档 )
Number of threads to use for background computations.
类型: int32
测试结果
官方建议优化至每个机器的CPU数量，由于每个机器都不太相同，因此没有进行测试。
2.3 global_sampling_ratio
参数解释
全局定位时，生成单个点位的速率
（官方文档）
Rate at which we sample a single trajectory’s nodes for global localization.
默认值 = 0.003
类型: double
测试结果（从右往左）
可以看到随着数值的变化，平均波动较大。其中，在0.001，0.002时平均误差处于较小位置，优化时可以考虑的值为0.001和0.002。
2.4 constraint_builder.sampling_ratio
参数解释
当增加的限制和潜在的限制比例低于该值时，会增加更多的限制。
（官方文档）
A constraint will be added if the proportion of added constraints to potential constraints drops below this number.
默认值 = 0.3
类型: double
测试结果（从右往左）
我们可以看到该参数在0.2-0.25左右平均误差处于相对低位。最小方差的位置在在0.25左右。低于0.2以后，平均误差及其方差达到变化很快到高位。因此推荐参数修改范围为0.2-0.3。
2.5 constraint_builder.min_score
参数解释
当匹配分数低于此值时，忽略该分数。当分数过低时，表明扫描结果和实际地图不匹配。
（官方文档）
Threshold for the scan match score below which a match is not considered. Low scores indicate that the scan and map do not look similar.
默认值 = 0.55
类型: double
测试结果（从左往右）
我们可以看到该参数在0.75-0.95（从左往右第二第三个点）左右误差的方差和最大误差处于相对低位。虽然在0.75时平均误差相对较高，但仍然推荐0.55-0.95为修改参数范围。高于1以后，各项值都上升的相对快速。
2.6 search_windows_sizes.linear_xy_search_window
参数解释
用于潜在对齐xy轴的扫描结果
（官方文档）
Linear search window in the plane orthogonal to gravity in which the best possible scan alignment will be found.
默认值 = 5
类型: double
测试结果（从左往右）
我们可以看到该参数在10-20（从左往右第一第二个点）左右误差的方差和最大误差上升较快，在10左右时处于较低点。因此推荐5-10为修改参数范围。
2.7 search_windows_sizes.linear_z_search_window
参数解释
用于潜在对齐z轴的扫描结果
（官方文档）
Linear search window in the gravity direction in which the best possible scan alignment will be found.
默认值 = 1
类型: double
测试结果（从左往右）
我们可以看到该参数在4（从左往右第二个点）以后误差的方差和最大误差上升较快，在4左右时仍处于较低点。因此推荐1-4为修改参数范围。
2.8 search_windows_sizes.angular_search_window
参数解释
用于潜在对齐旋转角的扫描结果
（官方文档）
Minimum angular search window in which the best possible scan alignment will be found.
默认值 = RAD(15)
类型: double
测试结果（从左往右）
我们可以看到该参数随着角度的增加，误差的方差和最大误差减小。推荐60左右为修改参数范围。
2.9 global_constraint_search_after_n_seconds
参数解释
提供全局闭环优化间隔的时间
（官方文档）
If for the duration specified by this option no global contraint has been added between two trajectories, loop closure searches will be performed globally rather than in a smaller search window.
默认值 = 10
类型: double
测试结果（从左往右）
我们可以看到该参数随着间隔时间的增加误差的方差和最大误差逐渐下降，但最大误差逐渐上升。在30-40时处于两者的平衡点。因此推荐30-40为修改参数范围。
2.10 ceres_scan_matcher.max_num_iterations
默认值 = 10
类型: int32
测试结果（从右往左）
我们可以看到该参数误差波动较大，平均误差大时，最大误差小，平均误差小时最大误差大。但在7左右处于相对平衡点，因此推荐7-10为修改参数范围。
3. 结论
optimize_every_n_nodes：推荐参数修改范围为30-70，官方文档中建议减少该值来提高速度，推荐30左右配置较为良好
MAP_BUILDER.num_background_threads：建议优化至每个机器的CPU线程数量
global_sampling_ratio：在0.001，0.002时平均误差处于较小位置，优化时可以考虑的值为0.001和0.002
constraint_builder.sampling_ratio：该参数在0.2-0.25左右平均误差处于相对低位。最小方差的位置在在0.25左右，推荐参数修改范围为0.2-0.3
constraint_builder.min_score：该参数在0.75-0.95误差的方差和最大误差处于相对低位，推荐0.75-0.95为修改参数范围
search_windows_sizes.linear_xy_search_window：该参数在10-20左右误差的方差和最大误差上升较快，在10左右时处于较低点，推荐5-10为修改参数范围
search_windows_sizes.linear_z_search_window：该参数在4以后误差的方差和最大误差上升较快，在4左右时仍处于较低点，推荐1-4为修改参数范围
search_windows_sizes.angular_search_window：该参数随着角度的增加，误差的方差和最大误差减小，推荐60左右为修改参数范围
global_constraint_search_after_n_seconds：该参数随着间隔时间的增加误差的方差和最大误差逐渐下降，但最大误差逐渐上升，在30-40时处于两者的平衡点。推荐30-40为修改参数范围
ceres_scan_matcher.max_num_iterations：该参数误差波动较大，平均误差大时最大误差小，平均误差小时最大误差大，在7左右处于相对平衡点，推荐7-10为修改参数范围