任务动机:分别对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种雷达数据与已有子地图匹配算法:
- CeresScanMatcher:通过迭代优化算法(利用Ceres实现),给定初始位姿,求解最优的位姿使得变换后雷达数据可以与子地图吻合得最好。这个算法相对较快。
- 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 步骤总结
- 下载安装相关工具
- 运行python自动记录脚本
- 运行Demo包
- 在自动记录脚本中输入 /map 和 /base_link
- 使用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以上的效果较为优秀。