Cartographer的前端参数设置和效果分析

任务动机：分别对cartographer的前端和后端进行运行时间分析，以确定cartographer的性能瓶颈和指导需要调整的参数。

任务描述：关掉后端，测试前端的运行时间。 然后再打开后端，测试整体运行时间，分析瓶颈是在前端还是在后端，以及每个阶段里面的耗时地方。此子任务是关掉后端，测试前端的运行时间。

1. cartographer算法流程介绍

1.1 数据预处理

• LaserScan数据的运动补偿

options.num_subdivisions_per_laser_scan = 10           // 一帧scan数据被拆成10部分，每部分按一个TimedPointCloudData消息发送
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 10  // 然后再把10个点云消息拼接起来（做运动补偿），作为当前帧输入

TRAJECTORY_BUILDER_2D.num_accumulated_range_data也可以用来把多个LIDAR传感器数据汇集在一起。

1.2 前端（Local SLAM，LocalTrajectoryBuilder）

前端的任务是将输入雷达数据与已有子地图做匹配，获得机器人位姿，以及将输入雷达数据更新到已有子地图上，或创建新的子地图。 前端做完后，则可以获得当前帧与相关子地图的相对位姿关系，这个关系将用于后端位姿图优化。

cartographer提供了2种雷达数据与已有子地图匹配算法：

1. CeresScanMatcher：通过迭代优化算法（利用Ceres实现），给定初始位姿，求解最优的位姿使得变换后雷达数据可以与子地图吻合得最好。这个算法相对较快。
2. RealTimeCorrelativeScanMatcher：当TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching设置为true时，将先采用这种匹配算法获得初始位姿， 然后再用CeresScanMatcher优化。

2. 测试方法

2.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.bag

demo包参数信息

包名	预计运行时间	包大小
b0-2014-08-14-13-23-01.bag	768 s	204 MB

2. 调整参数

根据官方文档google-cartographer-ros-readthedocs-io-en-latest.pdf在Chapter 5.3.1中提出的修改参数建议进行修改。

主要需要修改的文件在以下目录中

cartographer/configuration_files/trajectory_builder_2d.lua 

2.3 运行测试

运行我们开发的自动记录脚本

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包，不然会有信息缺失。

2.4 误差计算

2.4.1 使用 MATLAB 测量

基于邸老师提供的误差计算，制作了matlab脚本进行.xlsx文件的读取和计算。（参考仓库 Calculate Pose Error）

将csv.py输出文件作为该Script的输入端可以计算每个时刻的位移误差和旋转角度误差，可视化误差曲线和打印平均误差，作为cartographer的性能定量评测指标。

2.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 [差值]

2. 5 步骤总结

1. 下载安装相关工具
2. 运行python自动记录脚本
3. 运行Demo包
4. 在自动记录脚本中输入 /map 和 /base_link
5. 使用evo计算和绘制

3. 调参数据分析

3.1 TRAJECTORY_BUILDER_2D.voxel_filter_size

参数解读

2D激光测量的方式，导致近距离的激光点比较密集，而远处的点比较稀疏，为了解决这个问题，我们使用了这个voxel filter。

官方解释：

voxel filter that gets applied to the range data immediately after cropping.

参数类型：float

初始值 = 0.025

测试结果

0.05

  

我们可看到在这个参数下绘图比默认值有些偏差，但总体的地图形状和框架没有大大改变，在可接受范围内。

在此参数下，xyz的偏离在开始和结束时较为严重，中间相差较小

在整段的中都有较小但些许的偏离，在可接受范围内。

该参数下的误差计算结果如下：

项目名	值
max	0.183667
mean	0.004035
median	0.001529
min	0.000000
rmse	0.007315
sse	2.793442
std	0.006102

0.1

  

我们可看到在这个参数下绘图顶部位置发生相对于0.05时更大的差距，其余地方均在可接受范围内，但最后的参数可能需要小于这个值。

在此参数下，xyz的偏离在开始和结束时较为严重，中间相差较小

在整段的中都有较小但些许的偏离，在可接受范围内。

该参数下的误差计算结果如下：

项目名	值
max	0.109973
mean	0.005638
median	0.003214
min	0.000000
rmse	0.008845
sse	4.090870
std	0.006815

0.2

  

我们可看到在这个参数下绘图比默认值偏差较大，但总体的地图形状发生改变，顶部的的图连接处发生严重偏离，已超出可接受范围。

在此参数下，除了中间部分的偏离较小，xyz的偏离严重区域大约占2/3。

在整段的旋转位置均发生一定的偏离，其中yaw部分偏离较为严重，特别是在波动时，偏离严重

该参数下的误差计算结果如下：

项目名	值
max	0.268486
mean	0.004101
median	0.002119
min	0.000000
rmse	0.006984
sse	2.571082
std	0.005653

小结

综上，可见可以接受的调整参数范围在0.025 - 0.1之间。

误差随参数变化如下（从左到右）： 

可以看到平均误差在0.1 和0.2时发生起伏（从左往右第二个点和第三个点），虽然0.1的误差比0.2的更大，但由于0.2时绘图偏差严重我们仍然只能选择小于0.1的值。

3.2 submaps.resolution

参数解读

子地图的分辨率大小

官方解释：

Resolution of the map in meters.

参数类型：double

初始值 = 0.05

测试结果

0.08

  

我们可看到在这个参数下绘图比默认值有些偏差，但总体的地图形状和框架没有大的改变，除了左上角有在可接受范围内的偏离，大体的形状没有改变。

在此参数下，在x轴上的偏离在开始较为严重，中间相差较小，后部也有轻微偏离。y轴上偏离集中在后部，前部有轻微的偏离。

在整段的中都有较小但些许的偏离，在可接受范围内。

该参数下的误差计算结果如下：

项目名	值
max	0.103929
mean	0.005194
median	0.002934
min	0.000000
rmse	0.008193
sse	3.519628
std	0.006336

0.09

  

我们可看到在这个参数下绘图比默认值有些偏差，但总体的地图形状与0.08相差不大，但中部偏差相比0.08的参数有增大的趋势。

在此参数下，在x轴上的偏离在开始较为严重，中间相差较小，后部也有轻微偏离。y轴上前后部有一定的偏离。

在整段的中都有较小但些许的偏离，在可接受范围内。

该参数下的误差计算结果如下：

项目名	值
max	0.305804
mean	0.005792
median	0.003568
min	0.000000
rmse	0.008954
sse	4.134810
std	0.006828

0.1

  

我们可看到在这个参数下绘图比默认值偏差较大，总体的地图形状发生改变，上部的的图连接处发生严重偏离，已超出可接受范围。特别是上部三条线在本来不应该相交的区域相交

在此参数下，在x轴上的偏离在开始较为严重，中间相差较小，后部也有轻微偏离。y轴上前后部有一定的偏离。

在整段的旋转位置均发生一定的偏离，其中yaw部分偏离较为严重，特别是在波动时，偏离严重

该参数下的误差计算结果如下：

项目名	值
max	0.241574
mean	0.007085
median	0.004869
min	0.000000
rmse	0.010219
sse	5.420288
std	0.007364

小结

综上，可见可以接受的调整参数范围在0.05 - 0.1之间。

误差随参数变化如下（从左到右）： 

可以看到平均误差在0.1时达到顶峰（从左往右第三个点），代表此时也是误差接受范围的界限，从图像上看，0.1时绘制的轨迹已经发生较为严重的偏离，因此建议小于0.1较为良好。

3.3 submaps.num_range

参数解读

每个子地图数据的数量。

官方解释：

Number of range data before adding a new submap. Each submap will get twice the number of range data inserted: First for initialization without being matched against, then while being matched

参数类型：int32

初始值 = 90

测试结果

80

  

我们可看到在这个参数下绘图比默认值有轻微的偏差，完全在可接受范围内

在该参数下，x轴开始时有轻微偏差，但剩余部分均处于贴合状态；y轴则完全贴合

整段基本都完整贴合

该参数下的误差计算结果如下：

项目名	值
max	0.233314
mean	0.003938
median	0.001382
min	0.000000
rmse	0.007270
sse	2.758199
std	0.006111

50

  

我们可看到在这个参数下绘图比默认值有些偏差，但总体的地图形状相差不大，但左上部偏差相比参数在80时有增大的趋势。

在此参数下，在x轴上的偏离在开始较为严重，中间相差较小，后部也有轻微偏离。y轴上前后部有一定的偏离。

在整段的中都有较小但些许的偏离，在可接受范围内，其中yaw轴部分偏离较大

该参数下的误差计算结果如下：

项目名	值
max	0.180117
mean	0.007764
median	0.005045
min	0.000000
rmse	0.011578
sse	6.803687
std	0.008589

40

  

我们可看到在这个参数下绘图比默认值偏差较大，总体的地图形状发生改变，上部的的图连接处发生严重偏离，已超出可接受范围。特别是上部三条轨迹的交点产生相对严重的偏移，已超出可接受范围

在此参数下，在x轴上的偏离在开始较为严重，其余部分也有轻微偏离。y轴上前后部有一定的偏离，中部贴合度较高

在整段的旋转位置均发生一定的偏离，其中yaw轴部分偏离较大

该参数下的误差计算结果如下：

项目名	值
max	0.137777
mean	0.004189
median	0.001519
min	0.000000
rmse	0.007899
sse	3.194746
std	0.006696

小结

误差随参数变化如下（从右到左）： 

可以看到平均误差在60时达到顶峰（从左往右第四个点）；从图像上看，40/50时绘制的轨迹已经发生较大甚至严重的偏离，因此建议小于60较为良好。

综上，可见可以接受的调整参数范围在90 - 60之间，在80左右效果较为优秀。

3.4 TRAJECTORY_BUILDER_2D.max_range

参数解读

参数类型：float

初始值 = 30

测试结果

25

  

我们可看到在这个参数下绘图和默认值几乎没有偏差。

在该参数下，xy轴均贴合较为完整

整段基本都完整贴合

该参数下的误差计算结果如下：

项目名	值
max	0.230969
mean	0.007770
median	0.005289
min	0.000000
rmse	0.011348
sse	6.600054
std	0.008271

20

  

我们可看到在这个参数下绘图比默认值有些偏差，但总体的地图形状相差不大，但左上部有一定的偏差且相比参数在25时有增大的趋势。

在该参数下，x轴开始时偏差较为严重，y轴有轻微的偏差。

在整段的中都有较小的偏离，在可接受范围内。

该参数下的误差计算结果如下：

项目名	值
max	0.220744
mean	0.007775
median	0.005245
min	0.000000
rmse	0.011316
sse	6.441073
std	0.008222

15

  

我们可看到在这个参数下绘图比默认值偏差较大，总体的地图形状发生改变，上部的的图连接处发生严重偏离，已超出可接受范围。

在此参数下，在xy轴上的在开始和结束时发生较为严重偏离，其余部分也有轻微偏离。

在整段的旋转位置均发生一定的偏离，其中yaw轴部分在波动时偏离较大

该参数下的误差计算结果如下：

项目名	值
max	0.215509
mean	0.007691
median	0.005205
min	0.000000
rmse	0.011238
sse	6.444999
std	0.008194

小结

误差随参数变化如下（从右到左）： 

可以看到平均误差在15-25时较高（从左往右第二到四个点）；但由于参数在15时，轨迹偏离误差已经超出可接受范围，参数在10时，虽然平均误差小，但地图偏离更加严重。

综上，可见可以接受的调整参数范围在30 - 15之间，推荐20-25误差较小。

3.5 adaptive_voxel_filter.max_range

参数解读

官方解释：

Points further away from the origin are removed.

参数类型：float

初始值 = 50

测试结果

40

  

我们可看到在这个参数下绘图几乎没有误差，完全在可接受范围内。

在该参数下，xy轴均处于贴合状态。

整段基本都完整贴合。

该参数下的误差计算结果如下：

项目名	值
max	0.224719
mean	0.007630
median	0.005150
min	0.000000
rmse	0.011191
sse	6.442081
std	0.008186

30

  

我们可看到在这个参数下绘图有轻微误差，完全在可接受范围内。

在此参数下，在xy轴上均有有轻微的偏离。

在该参数下，三个旋转轴几乎没有的偏离。

该参数下的误差计算结果如下：

项目名	值
max	0.150884
mean	0.003797
median	0.001305
min	0.000000
rmse	0.006991
sse	2.563463
std	0.005871

20

  

我们可看到在这个参数下绘图比默认值偏差开始增大，总体的地图形状发生轻微改变，主要误差区域集中在上部，仍在可接受范围之内。

在此参数下，在x轴上的偏离在开始较为严重，其余部分也有轻微偏离。y轴上前后部有一定的偏离，中部贴合度较高

在整段的旋转位置的偏离细微。

该参数下的误差计算结果如下：

项目名	值
max	0.171928
mean	0.007794
median	0.005277
min	0.000000
rmse	0.011276
sse	6.504717
std	0.008149

10

  

我们可看到在这个参数下绘图比默认值偏差过大，总体的地图形状发生改变，上部的的图连接处发生严重偏离，已超出可接受范围。特别是上部三条轨迹的交点产生相对严重的偏移。

在此参数下，在xy轴上的偏离在开始和结束时严重，中部有轻微误差。

在整段的旋转位置均发生一定的偏离，部分波动区域误差严重。

该参数下的误差计算结果如下：

项目名	值
max	0.275181
mean	0.007700
median	0.005098
min	0.000000
rmse	0.011510
sse	6.750833
std	0.008555

小结

误差随参数变化如下（从右到左）： 

可以看到平均误差在30时达到最小值（从左往右第三个点）；从图像上看，20/10时绘制的轨迹逐渐发生严重的偏离，因此建议小于20较为良好。

综上，可见可以接受的调整参数范围在50 - 20之间，在30左右效果较为优秀。

3.6 adaptive_voxel_filter.max_length

参数解读

voxel边缘的最大长度

官方解释：

‘max_length’ of a voxel edge.

参数类型：float

初始值 = 0.5

测试结果

1

  

我们可看到在这个参数下绘图几乎没有误差，完全在可接受范围内。

在该参数下，xy轴均处于贴合状态。

整段基本都完整贴合。

该参数下的误差计算结果如下：

项目名	值
max	0.305048
mean	0.007493
median	0.004930
min	0.000000
rmse	0.011186
sse	6.483341
std	0.008305

8

  

我们可看到在这个参数下绘图几乎没有误差，完全在可接受范围内。

在该参数下，xy轴均处于贴合状态。

整段基本都完整贴合。

该参数下的误差计算结果如下：

项目名	值
max	0.176083
mean	0.007833
median	0.005356
min	0.000000
rmse	0.011384
sse	6.639561
std	0.008261

小结

误差随参数变化如下（从右到左）： 

可以看到平均误差在2-4时达到最小值（从左往右第二，三个点）；从图像上看，各参数图像的误差都在可接受范围内。

综上，可见可以接受的调整参数范围在0.5 - 4之间，在2 - 4左右效果较为优秀。

3.7 adaptive_voxel_filter.min_num_points

参数解读

如果点的数量超出这个值则会减小voxel的长度来达到该值

官方解释：

If there are more points and not at least ‘min_num_points’ remain, the voxel length is reduced trying to get this minimum number of points.

参数类型：float

初始值 = 200

测试结果

100

  

我们可看到在这个参数下绘图有一定的误差，尤其时上半部分的交点位置产生便宜。

在该参数下，xy轴的开头和结尾有一定偏移，其余位置基本贴合。

整段有轻微偏移，波动处误差稍大。

该参数下的误差计算结果如下：

项目名	值
max	0.250011
mean	0.004341
median	0.001967
min	0.000000
rmse	0.007500
sse	2.954390
std	0.006116

50

  

我们可看到在这个参数下绘图比默认值偏差开始增大，总体的地图形状发生改变，主要误差区域集中在上部，超出可接受范围之内。

在该参数下，xy轴的开头和结尾有严重误差，其余位置基本贴合。

整段有偏移，相比100时来的更大，波动处误差稍大。

该参数下的误差计算结果如下：

项目名	值
max	0.279126
mean	0.007536
median	0.004899
min	0.000000
rmse	0.011339
sse	6.583158
std	0.008472

25

  

我们可看到在这个参数下绘图比默认值偏差严重，总体的地图形状发生改变，超出可接受范围之内。

在此参数下，在xy轴上的误差严重，其余部分也有轻微偏离。

在整段的旋转位置的偏离严重，在波动处尤为明显。

该参数下的误差计算结果如下：

项目名	值
max	0.122127
mean	0.007786
median	0.005237
min	0.000000
rmse	0.011408
sse	6.645498
std	0.008337

总结

误差随参数变化如下（从右到左）： 

可以看到平均误差在100时达到最小值（从左往右第三个点）；从图像上看，100时绘制的轨迹逐渐发生偏离，如果值小于100测偏离更为严重，因此建议大于100较为良好。

综上，可见可以接受的调整参数范围在100 - 200之间，100以上的效果较为优秀。

4. 总结

TRAJECTORY_BUILDER_2D.voxel_filter_size：平均误差在0.1 和0.2时发生起伏，虽然0.1的误差比0.2的更大，但由于0.2时绘图偏差严重，建议选择小于0.1的值。
submaps.resolution：平均误差在0.1时达到顶峰，建议小于0.1较为良好。
submaps.num_range_data：可以接受的调整参数范围在90 - 60之间，在80左右效果较为优秀。
TRAJECTORY_BUILDER_2D.max_range：可以接受的调整参数范围在30 - 15之间，推荐20-25误差较小。
adaptive_voxel_filter.max_range：可以接受的调整参数范围在50 - 20之间，在30左右效果较为优秀。
adaptive_voxel_filter.max_length：可以接受的调整参数范围在0.5 - 4之间，在2 - 4左右效果较为优秀。
adaptive_voxel_filter.min_num_points：可以接受的调整参数范围在100 - 200之间，100以上的效果较为优秀。