Cartographer调参与系统资源使用情况分析
任务动机：通过Cartographer调参，达到在Rockchip rk3399开发板上实时和精度兼顾的效果。
任务描述：参照Cartographer官方调参文档，通过逐个调参，记录真实机器人建立的地图和系统资源消耗。
1. 硬件及OS信息
| name | type |
|---|---|
| CPU | RK3399 双核Cortex-A72(大核)+四核Cortex-A53(小核) |
| Kernel | 4.4.179 |
| ROS | Meloc |
| lidar | ToF |
| bag | Speed 0.266 m/s Turn 1.0 Rad/s |
使用预先录制好的bag，然后用同一个bag对比不同的参数效果
2. 参数效果
评判调参效果，从两方面进行：
- 建图中的优化实时性，即在建图中纠偏优化的实时速度；
- 建图完成后的优化速度或者优化时间。
建图完成以后，地图的保存分成三步，写成一个脚本文件，这里并没有用通用变量，用起来需要进入脚本修改名称。使用cartographer_ros/offline_node 进行参数粗调，找到建图质量较好的地图后，记录实时的CPU使用情况。
2.1 官方给出的调参教程
### Low latency 调整
#### local
- 提高 voxel_filter_size
- 提高 submaps.resolution
- 降低 min_num_points max_range 提高 max_length
- 降低 max_range
- 降低 submap.num_range_data
#### global
- 降低 optimize_every_n_nodes
- 提高 num_background_threads <= CPU 核数
- 降低 linear_xy_search_window linear_z_search_window angular_search_window
- 提高global_constraint_search_after_n_seconds
- 降低max_num_iterations2.2 参数调整
第一个调整的参数推荐 MAP_BUILDER.num_background_threads 先明确预留的CPU资源，在这个基础上进行调整。
2.2.1 Local SLAM(optimize_every_n_nodes = 0)
a.submap.num_range_data
构建子图需要的scan数据量
- value = 90(default)
过程效果 = 1
最终效果 = 4
CPU使用情况
- value = 10
过程中
优化后
CPU
- value = 50
过程中
优化后
- value = 30
过程中
优化后
CPU
- value = 70
过程中
优化后
CPU
- value = 40
优化后
- value = 60
过程中 
优化后 
Result:
可以看出，num_range_data设置的值与CPU有这样一种关系，值小(10)，CPU使用率比较稳定，整体偏高，值大时，CPU短暂爆发使用(插入子图的时候)，平时使用率低，呈现极大的波动状态。
num_range_data表示多少个scan来构成submap，这个值的设定需要根据laser和运动速度进行具体的调整。
b. real_time_correlative_scan_matcher.rotation_delta_cost_weiht
-
value = 0.0
- 未进行优化前
finish_trajectory 后，及完成优化效果均相同
CPU
- value = 0.1
(default)
original 
finish_trajectory and optimized 
CPU
-
value = 0.3
original 
finish_trajectory & optimized 
CPU
-
0.6
original 
finish_trajectory & optimized 
-
1.0
original
optimized
CPU
-
2.0
original 
finish_trajectory & unoptimized 
CPU
Results:
从CPU使用率来看，该参数并不会产生明显的影响，主要影响的是过程中的效果，间接会影响最后的优化时间长
2.2.2 Global SLAM
调整scan_matcher中的求解方法
a. bool use_online_correlative_scan_matching
选择是否先求解online scan matching，然后用correlative scan matcher为Ceres求解器产生一个好的初始解
- value = false (default)
| 完成优化时间 | 过程效果(1-4/差-佳) | 最终效果(1-4/差-佳) |
|---|---|---|
| 230 | 2 | 4 |
最终优化效果于使用初值相同。
- value = true
| 完成优化时间 | 过程效果(1-4/差-佳) | 最终效果(1-4/差-佳) |
|---|---|---|
| 260 | 3 | 4 |
b. Ceres.POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.branch_and_bound_depth
branch and bound是优化器中fast_correlative_matcher搜索方法，界定分支法，求解问题构成一个搜索树，depth是构造树的深度。
- value = 7 （default）
| 完成优化时间 | 过程效果(1-4/差-佳) | 最终效果(1-4/差-佳) |
|---|---|---|
| 230 | 3 | 4 |
过程中
优化后
-
value = 3
| 完成优化时间 | 过程效果(1-4/差-佳) | 最终效果(1-4/差-佳) |
|---|---|---|
| so long | 优化时间太长，放弃 | 放弃 |
-
value = 5
| 完成优化时间 | 过程效果(1-4/差-佳) | 最终效果(1-4/差-佳) |
|---|---|---|
| 1316 | 2 | 4 |
优化完成后
- value = 9
| 完成优化时间 | 过程效果(1-4/差-佳) | 最终效果(1-4/差-佳) |
|---|---|---|
| 245 | 3 | 4 |
过程1 
优化完成后 
- value = 8
| 完成优化时间 | 过程效果(1-4/差-佳) | 最终效果(1-4/差-佳) |
|---|---|---|
| 142 | 4 | 4 |
过程1 
优化完成后 
这里走了一个大坑，在从机使用bag play，省掉了一个ssh到主机的过程，结果网络延迟造成了结果不稳定。
c. real_time_correlative_scan_matcher.real_time_correlative_scan_matcher.linear_search_window
线距离搜索框，在这个框的大小内，搜索最佳scan匹配.
-
value = 0.1
| 完成优化时间 | 过程效果(1-4/差-佳) | 最终效果(1-4/差-佳) |
|---|---|---|
| 271 | 3 | 4 |
-
value = 0.9
| 完成优化时间 | 过程效果(1-4/差-佳) | 最终效果(1-4/差-佳) |
|---|---|---|
| unknown | 实时效果极差，舍弃 | unknown |
-
value = 0.5
| 完成优化时间 | 过程效果(1-4/差-佳) | 最终效果(1-4/差-佳) |
|---|---|---|
| unknown | 实时效果差，舍弃 | unknown |
-
value = 0.3
| 完成优化时间 | 过程效果(1-4/差-佳) | 最终效果(1-4/差-佳) |
|---|---|---|
| unknown | 实时效果不佳，延迟感太强，舍弃 | unknown |
-
value=0.05
| 完成优化时间 | 过程效果(1-4/差-佳) | 最终效果(1-4/差-佳) |
|---|---|---|
| 383 | 4 | 4 |
减小该参数可以增强实时的建图效果，降低闭环优化的效果，形成闭环时，产生的重影较多
d. real_time_correlative_scan_matcher.real_time_correlative_scan_matcher.angular_search_window
同上，角度搜索框的大小
-
value = 20°(default)
| 完成优化时间 | 过程效果(1-4/差-佳) | 最终效果(1-4/差-佳) |
|---|---|---|
| 271 | 3 | 4 |
-
value = 40
| 完成优化时间 | 过程效果(1-4/差-佳) | 最终效果(1-4/差-佳) |
|---|---|---|
| 太长，不等了 | 1 | 4 |
过程中出现
-
value = 10
| 完成优化时间 | 过程效果(1-4/差-佳) | 最终效果(1-4/差-佳) |
|---|---|---|
| 409 | 3 | 3 |
过程中 
过程中2 
优化后
d.real_time_correlative_scan_matcher( translation_delta_cost_weight)/(rotation_delta_cost_weight)
这两个参数相当于最小化误差函数中的权重值，两者的比值，决定了更侧重与平移和旋转中的哪部分
这个值大小的设定，可以参照local latency rotation_delta_cost_weight的修改
-
value = 1e-1 / 1e-1
| 完成优化时间 | 过程效果(1-4/差-佳) | 最终效果(1-4/差-佳) |
|---|---|---|
| 271 | 3 | 4 |
-
value = 1e-1 / 2e-1
| 完成优化时间 | 过程效果(1-4/差-佳) | 最终效果(1-4/差-佳) |
|---|---|---|
| long ... | 2 | 4 |
- value = 5e-2/1e-1
| 完成优化时间 | 过程效果(1-4/差-佳) | 最终效果(1-4/差-佳) |
|---|---|---|
| long... | 2.5 | 4 |
3. 其他相似参数
| parameters | type | annotation |
|---|---|---|
| weight | double | 优化轨迹时的重点优化部分，相对translation和rotation的比例 |
| max_num_iterations | int | 优化迭代步数 |
| linear_search_window | int | scan匹配的帧数范围 |
| odometry_rotation/translation weight | double | 调整比例，改变优化误差函数的着重部分 |
| huber_scale | double | 鲁棒核函数，去噪 |
4. 参考
1.https://google-cartographer-ros.readthedocs.io/en/latest/tuning.html
2.https://google-cartographer-ros.readthedocs.io/en/latest/algo_walkthrough.html
3.https://google-cartographer.readthedocs.io/en/latest/configuration.html