ROS2で3D LiDARを用いたGraph SLAMによる三次元地図作成

ROS2で3D LiDARできるパッケージが無かったので、今回はROS2で3D LiDARを使用したGraph SLAMのプログラムを書いて三次元地図を作りました！

書いたコードはGithubにあります。
https://github.com/rsasaki0109/graphslam_ros2
<a href="https://github.com/rsasaki0109/graphslam_ros2"><img src="https://github-link-card.s3.ap-northeast-1.amazonaws.com/rsasaki0109/graphslam_ros2.png" width="460px"></a>

ループクローズにより修正された経路
![mapping_with_loopclosure.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/229952/29857791-2951-b3ca-7ffc-dd1e6451b3d3.png)

地図
![map.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/229952/9e9e6573-491b-e79f-78b9-58c2b1aa481b.png)


# 事前知識  
graphslam_ros2は逐次的なNDT/GICP-スキャンマッチングによるフロントエンドと一定周期で実行されるGraph SLAMによるバックエンドによって成り立っています。

スキャンマッチングに関しては以下のスライドを読めばわかると思います。
[Scan Matching  自己位置推定手法](https://web.wakayama-u.ac.jp/~nakajima/SelfDrivingSystem/assets/pdf/method_pmv_03.pdf)
[LiDAR-SLAM チュートリアル資料](https://www.slideshare.net/FujimotoKeisuke/lidarslam-99008083)

NDT scan matchingについて詳しくは以下を参照ください。(GICPに関して、解説してる日本語記事はわかりませんが、元論文が比較的簡単に読めると思います。）
[gggggraziegrazie NDT( Normal Distributions Transform )](
http://graziegrazie.hatenablog.com/entry/2018/07/01/005613)
[NDTスキャンマッチング 第1回3D勉強会@PFN 2018年5月27日](
https://www.slideshare.net/YukiKitsukawa/ndt-13dpfn-2018527)

Graph SLAMに関しては以下の解説論文に詳しく書いてあります。
[移動ロボットの環境認識 —地図構築と自己位置推定]
(https://www.jstage.jst.go.jp/article/isciesci/60/12/60_509/_pdf)  
より詳しくは『SLAM入門』を参照ください。
# 環境
ubuntu：18.04
ros1：melodic
ros2：dashing


# アルゴリズムの説明
- frontend(scan-matcher)   
以前の複数の入力点群と現在の入力点群で逐次スキャンマッチングをします。また、一定の距離毎にループ探索の候補となる部分地図(とその中心位置)を生成し、
保存していきます。

- backend(graph-based-slam)  
一定周期で直近の候補点と今までの候補点全てでndtスキャンマッチングを行い、マッチングスコアが閾値以下ならそこでループ制約を生成し、ポーズ最適化を行います。

# パッケージ
frontendにgicp/ndt scan matching、backendにg2oによるgraph-based slamを使っています.
## 特徴
・[ndt_omp](https://github.com/koide3/ndt_omp)によるOpenMPを使用した高速なスキャンマッチング
・g2oによるポーズ調整
・スキャンマッチングの初期値をオドメトリで補間
・(回転式LiDARのみ)9軸IMUで歪み補正

## 入出力

### フロントエンド(scan-matcher) 
- input   
/input_cloud  (sensor_msgs/PointCloud2) 
/tf(from "base_link" to LiDAR's frame)
/initial_pose  (geometry_msgs/PoseStamed)(optional) 
/imu  (sensor_msgs/Imu)(optional) 
/odom  (nav_msgs/Odometry)(optional)  

- output  
/current_pose (geometry_msgs/PoseStamped) 
/map  (sensor_msgs/PointCloud2) 
/path  (nav_msgs/Path) 
/tf(from "map" to "base_link") 
/map_array(graphslam_ros2_msgs/MapArray)

scan-matcherはバックエンドなしでも使えます。

### バックエンド(graph-based-slam)
- input  
/map_array(graphslam_ros2_msgs/MapArray)
- output  
/modified_map_array(graphslam_ros2_msgs/MapArray)  
/modified_path  (nav_msgs/Path)   
/modified_map  (sensor_msgs/PointCloud2)  

またgraph-based-slamは `pose_graph.g2o` と `map.pcd` をループ閉じ込み時または下記のサービスコール時に出力します。

```
ros2 service call /map_save std_srvs/Empty
```
## パラメーター

- フロントエンド(scan-matcher) 

|Name|Type|Default value|Description|
|---|---|---|---|
|registration_method|string|"NDT"|"NDT" か "GICP"を選べます|
|ndt_resolution|double|5.0|ボクセルの解像度[m]|
|ndt_num_threads|int|0|ndtに使うスレッド数(0にすると使用できる最大のスレッド数を使います)|
|trans_for_mapupdate|double|1.5|map updateに必要な移動距離[m]|
|vg_size_for_input|double|0.2|input cloudのdown sample size[m]|
|vg_size_for_map|double|0.05|map cloudのdown sample size[m]|
|scan_max_range|double|100.0|使用する入力点群の最大距離[m]|
|scan_min_range|double|1.0|使用する入力点群の最小距離[m]|
|scan_periad|double|0.1|LiDARの回転周期[sec]|
|map_publish_period|double|15.0|点群地図のpuplish周期[sec]|
|num_targeted_cloud|int|10|registrationのtargetに使用する点群の数|
|set_initial_pose|bool|false|paramファイルから初期位置(map原点)を決めるかどうか|
|initial_pose_x|double|0.0|初期位置のx座標[m]|
|initial_pose_y|double|0.0|初期位置のy座標[m]|
|initial_pose_z|double|0.0|初期位置のz座標[m]|
|initial_pose_qx|double|0.0|初期位置のqx|
|initial_pose_qy|double|0.0|初期位置のqy|
|initial_pose_qz|double|0.0|初期位置のqz|
|initial_pose_qw|double|1.0|初期位置のqw|
|use_imu|bool|false|LiDARの歪み補正にimuを使うかどうか|
|use_odom|bool|false|スキャンマッチングの初期姿勢値にodomを使うかどうか|
|debug_flag|bool|false|スキャンマッチング結果の情報を表示するかどうか|

- バックエンド(graph-based-slam)

|Name|Type|Default value|Description|
|---|---|---|---|
|ndt_resolution|double|5.0|ndtの解像度[m]|
|ndt_num_threads|int|0|ndtに使うスレッド数(0にすると使用できる最大のスレッド数を使います)|
|voxel_leaf_size|double|0.2|input cloudのdown sample size[m]|
|loop_detection_period|int|1000|ループ探索を行う周期[ms]|
|threshold_loop_clousure_score|double|1.0| loop clousureしたと判定するためのndtのマッチングスコアの閾値|
|distance_loop_clousure|double|20.0| 候補点をloop clousureの探索対象にするために候補点からどれくらい離れるか[m]|
|range_of_searching_loop_clousure|double|20.0|現在地からloop clousureの探索範囲[m]|
|search_submap_num|int|2|registrationに使用する再訪点の前後のサブマップ点の数|

## デモ

デモデータは[hdl_graph_slam](https://github.com/koide3/hdl_graph_slam)にあるROS1の`hdl_400.bag` を使います。
後は以下のコマンドを順次実行してください。

```
rviz2 -d src/graphslam_ros2/scanmatcher/config/mapping.rviz 
```

```
ros2 launch lidarslam lidarslam.launch.py
```


```
ros2 bag play -s rosbag_v2 hdl_400.bag 
```


結果は以下のようになります！うまくループが閉じていることが確認できますね！

![mapping_with_loopclosure.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/229952/29857791-2951-b3ca-7ffc-dd1e6451b3d3.png)
Green: path with loopclosure, Yellow: path without loopclosure

地図
![map.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/229952/9e9e6573-491b-e79f-78b9-58c2b1aa481b.png)



# さいごに
とりあえず動いて良かったです。
普段使ってるOSSのありがたさを実感しました・・・