# Cartographer_paramers_with_robot

**Repository Path**: sirius-j/Cartographer_paramers_with_robot

## Basic Information

- **Project Name**: Cartographer_paramers_with_robot
- **Description**: Cartographer真机调参及资源使用
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 11
- **Created**: 2021-12-15
- **Last Updated**: 2021-12-15

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# Cartographer真机调参及资源使用

动机：调整Cartographer主要参数，达到在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. 参数效果
 评判调参效果，从两方面进行：
 1. 建图中的优化实时性，即在建图中纠偏优化的实时速度；
 2. 建图完成后的优化速度或者优化时间。

建图完成以后，地图的保存分成三步，写成一个[脚本文件](./maps/save_map.sh)，这里并没有用通用变量，用起来需要进入脚本修改名称。使用cartographer_ros/offline_node 进行参数粗调，找到建图质量较好的地图后，记录实时的CPU使用情况。

---- `多图预警` ----


### 2.1 [官方给出的调参教程]() 

```shell
### 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_iterations
```

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

![90](./pics/submap_num_range_data/90.png)

最终效果 = 4 

![90-2](./pics/submap_num_range_data/90-finishedTraj.png)

CPU使用情况

![90](./maps/submap_num_range_data/90.png)



- value = 10

过程中

![10](./pics/submap_num_range_data/10.png)

优化后

![10-afterAPeriod](./pics/submap_num_range_data/10-afterAPeriod.png)

CPU

![10](./maps/submap_num_range_data/10.png)

- value = 50

过程中

![50](./pics/submap_num_range_data/50.png)

优化后

![50-afterAPeriod](./pics/submap_num_range_data/50-afterAPeriod.png)

CPU

[lack]  

- value = 30

过程中

![30](./pics/submap_num_range_data/30.png)

优化后

![30-afterAPeriod](./pics/submap_num_range_data/30-afterAPeriod.png)

CPU

![30](./maps/submap_num_range_data/30.png)

- value = 70

过程中

![70](./pics/submap_num_range_data/70.png)

优化后

![70-afterFinishedTraj](./pics/submap_num_range_data/70-afterFinishedTraj.png)

CPU

![70](./maps/submap_num_range_data/70.png)

- value = 40
  - 优化后

![40-finishedTraj](./pics/submap_num_range_data/40-finishedTraj.png)

- value = 60
  - 过程中
![60](./pics/submap_num_range_data/60.png)
  - 优化后
![60-finishedTraj](./pics/submap_num_range_data/60-finishedTraj.png)

>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 后，及完成优化效果均相同
  
  ![0](./pics/rotation_delta_cost_weight/0.png)
  
  	- CPU

![0](maps/rotation_delta_cost_weight/0.png)

- value = 0.1 `(default)`
  - original
	![0.1](./pics/rotation_delta_cost_weight/0.1.png)
  - finish_trajectory and optimized
  ![0.1-optimized](./pics/rotation_delta_cost_weight/0.1-optimized.png)
  - CPU

![1e-1](maps/rotation_delta_cost_weight/1e-1.png)

- value = 0.3
  - original
  ![0.3](./pics/rotation_delta_cost_weight/0.3.png)
  - finish_trajectory & optimized
  ![0.3-optimized](./pics/rotation_delta_cost_weight/0.3-optimized.png)
  - CPU
  
  ![0.3](./maps/rotation_delta_cost_weight/0.3.png)
  
- 0.6
  - original
  ![0.6](./pics/rotation_delta_cost_weight/0.6.png)
  - finish_trajectory & optimized
  ![0.6-optimized](./pics/rotation_delta_cost_weight/0.6-optimized.png)
  
- 1.0
  
  - original
  ![1.0](./pics/rotation_delta_cost_weight/1.0.png)
  - optimized
  ![1.0-optimized](./pics/rotation_delta_cost_weight/1.0-optimized.png)
  - CPU
  
  ![1.0](./maps/rotation_delta_cost_weight/1.0.png)
  
- 2.0
  
  - original
  ![2.0](./pics/rotation_delta_cost_weight/2.0.png)
  - finish_trajectory & unoptimized
  ![2.0-optimized](./pics/rotation_delta_cost_weight/2.0-optimized.png)
  - CPU
  
  ![2.0](./maps/rotation_delta_cost_weight/2.0.png)
  
  > 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|

![use_online_true](./pics/use_online_true.png)  

**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 |

	- 过程中
<img src="./pics/branch_and_bound/b7.png" alt="b7" style="zoom:87%;" />

	- 优化后
![7-1](./pics/branch_and_bound/7-1.png)

- value = 3 
| 完成优化时间 | 过程效果(1-4/差-佳) | 最终效果(1-4/差-佳) |
| ------------ | -------- | --- |
| so long | 优化时间太长，放弃 | 放弃 |

- value = 5 
| 完成优化时间 | 过程效果(1-4/差-佳) | 最终效果(1-4/差-佳) |
| ------------ | -------- | --- |
| 1316 | 2 | 4 |

	- 优化完成后

![5-1](./pics/branch_and_bound/5-1.png)

- value = 9

| 完成优化时间 | 过程效果(1-4/差-佳) | 最终效果(1-4/差-佳) |
| ------------ | -------- | --- |
| 245 | 3 | 4 |

过程1 
![9-1](./pics/branch_and_bound/9-1.png)
优化完成后
![9-2--146](./pics/branch_and_bound/9-2--146.png)

- value = 8

| 完成优化时间 | 过程效果(1-4/差-佳) | 最终效果(1-4/差-佳) |
| ------------ | -------- | --- |
| 142 | 4 |4|

过程1 
![8-1](./pics/branch_and_bound/8-1.png)
优化完成后
![8-2](./pics/branch_and_bound/8-2.png)

> 这里走了一个大坑，在从机使用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 |
	- 过程中出现

![40-1](./pics/angular_search_window/40-1.png)

- value = 10
| 完成优化时间 | 过程效果(1-4/差-佳) | 最终效果(1-4/差-佳) |
| ------------ | -------- | --- |
| 409 | 3 | 3 |

 - 过程中
![10-1](./pics/angular_search_window/10-1.png)

 - 过程中2
![10-2](./pics/angular_search_window/10-2.png)

 - 优化后


![10-3](./pics/angular_search_window/10-3.png)

**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 |
![0.5-1](./pics/rotation_and_translation/0.5-1.png)

- value = 5e-2/1e-1
| 完成优化时间 | 过程效果(1-4/差-佳) | 最终效果(1-4/差-佳) |
| ------------ | -------- | --- |
| long... | 2.5 | 4 |
![0.5-s-1](./pics/rotation_and_translation/0.5-s-1.png)

## 其他相似参数

| parameters                   | type   | annotation           |
| --------- | ------ | -------- |
| weight           | double | 优化轨迹时的重点优化部分，相对translation和rotation的比例 |
| max_num_iterations                   | int    | 优化迭代步数  |
| linear_search_window                 | int    | scan匹配的帧数范围  |
| odometry_rotation/translation weight | double | 调整比例，改变优化误差函数的着重部分    |
| huber_scale     | double | 鲁棒核函数，去噪    |

## 参考

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