ROS2でNDT scan matchingによるマッピングとマップマッチングによる自己位置推定[SLAM]


# はじめに
こんにちは。
本記事はROS2 アドベントカレンダーの12日目です。遅れて申し訳ありません。
前回は@smakitaさんの[「WSL に ROS2 をインストールします」](https://qiita.com/smakita/items/3d704f694f531acd0168)、
次回は@youtalkさんの[「QoSプロファイルの実践と課題」](https://ros.youtalk.jp/2019/12/13/qos-profile.html)です。

今回は、以前ROS2で3D-SLAMできそうなソフトウェアを探したんですがなさそうで、[pcl(point cloud library)でNDT(Normal Distributions Transform) scan matchingによるマップマッチングによる自己位置推定のコード](https://github.com/OUXT-Polaris/pcl_apps)があったので、それを基に勉強がてらマッピングとマップマッチングによる自己位置推定のコードを書いて動かしました！

ちなみに、ROS2で2D-SLAMはROBOTIS社のロボットturtlebot3のROS2チュートリアルで実機・simulationともに簡単にできます。こちらのSLAMはgoogle cartographerを使っています。なお、これによる3D-SLAMの移植はされていません。
http://emanual.robotis.com/docs/en/platform/turtlebot3/overview/
また、gmappingも移植している人がいて、[その移植版を使っている人(リンク先Qiita記事)](https://qiita.com/l1sum/items/97628c307ac19ae69c42)もいるようです。
https://github.com/Project-MANAS/slam_gmapping
他にも[slam_toolbox](https://github.com/SteveMacenski/slam_toolbox)等があります。

# 環境
ubuntu：18.04
ros1：melodic
ros2：dashing

PCスペック
メインメモリ：16GB
クロック数：3.5GHz(The governor "performance")
キャッシュメモリ：8MB
コア：4コア
# NDT scan matchingとは

NDT scan matchingについて詳しくは以下を参照ください。

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

説明は一番上のgraizegrazieさんさんのブログ記事から引用します。
スキャンマッチングの説明。
>NDT[1]とは、２つの点群のマッチングをさせる手法の１つです。点群マッチングとは、ある点群（参照点群と呼ぶ）に対し、ある点群（入力点群と呼ぶ）がどれだけ重なっているか、重ねるためには点群をどの様に移動させるべきかを判断するための手法です。

NDTの説明
>これに対してNDTは、ボクセルやグリッドを使って点群が存在する空間を等間隔に分割するし、ボクセル又はグリッド毎に求めた正規分布を使って点群の表現、マッチングを行うため、計算量を削減することができます。

pclのndtのソースは以下です。ソースに参考文献も書いてあります。
https://github.com/PointCloudLibrary/pcl/blob/master/registration/include/pcl/registration/impl/ndt.hpp
# ソース
プログラムは以下のソースを参考に書きました。
https://github.com/OUXT-Polaris/pcl_apps

書いたコードはGithubにあります。
<a href="https://github.com/rsasaki0109/pcl_apps"><img src="https://github-link-card.s3.ap-northeast-1.amazonaws.com/rsasaki0109/pcl_apps.png" width="460px"></a> 
マッピング
https://github.com/rsasaki0109/pcl_apps/tree/master/pcl_apps/src/mapping
マッチング(こっちは基のコードを微修正しただけ)
https://github.com/rsasaki0109/pcl_apps/tree/master/pcl_apps/src/matching
# 実験

## マッピング
### 設定
ndtのresolutionは5.0(屋外環境での値の目安は5~10mくらい)、
https://gitlab.com/autowarefoundation/autoware.ai/core_perception/blob/master/lidar_localizer/nodes/ndt_mapping/ndt_mapping.cpp#L116
入力点群のvoxel_gridのサイズは以下を参考に0.2にしました。
https://gitlab.com/autowarefoundation/autoware.ai/core_perception/blob/master/lidar_localizer/nodes/ndt_mapping/ndt_mapping.cpp#L121
### 実行手順
rosbagデータは[AutowareのROS1のdemoデータ](https://gitlab.com/autowarefoundation/autoware.ai/autoware/wikis/ROSBAG-Demo)を使います。
ROS2でROS1のbagデータを再生するにはrosbag2に加え、rosbag2_bag_v2をインストールする必要があります。aptで入ります。
ちなみに3D LiDAR SLAMと言ったらvelodyneが有名ですが、velodyneのROS2ドライバーは現状以下の様らしいです。

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">ros2のvelodyneドライバー、unstableすぎる<br>30秒以上もたない</p>&mdash; 片岡大哉 @ 技術書典 い23 (@hakuturu583) <a href="https://twitter.com/hakuturu583/status/1188848039177252864?ref_src=twsrc%5Etfw">October 28, 2019</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>
また、他のドライバーでは以下が見つかりました(動作未確認)
https://github.com/SteveMacenski/ros2_ouster_drivers    
                 




demoデータのダウンロード

```
wget http://db3.ertl.jp/autoware/sample_data/sample_moriyama_150324.tar.gz
tar zxfv sample_moriyama_150324.tar.gz
```
  
ノードの起動

```
ros2 launch pcl_apps ndt_mapping.launch.py
```
(ちなみにros2 run pcl_apps ndt_mapping_node ndt_mapping/input:=/points_raw
でも起動できますが、ros dashingではros2 runでパラメータを渡せないので面倒です。次の世代のros eloquentだとできるようになります)

位置姿勢初期値を指定します。

```
ros2 topic pub ndt_mapping/initial_pose geometry_msgs/PoseStamped '{ pose: {position: {x: 0, y: 0}, orientation: {z: 0, w: 1}}}' --once
```

rosbag再生。

```
ros2 bag play -s rosbag_v2 sample_moriyama_150324.bag 
```
### 結果
mappingした結果がこちらです。
![mapping.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/229952/e0be6a4c-0b54-ae26-66f5-27ac578b31be.png)
大丈夫そうですね！

## マップマッチングによる自己位置推定
マップマッチングは、[自己位置推定パッケージmcl_3dl](https://github.com/at-wat/mcl_3dl)のデモデータを使って自己位置推定します。
### 設定
以下を参考にndtのresolutionは0.2,入力点群のvoxel_gridのサイズは0.1にしました。
https://github.com/at-wat/mcl_3dl/blob/master/config/test_localization.yaml
### 実行手順
demoデータのダウンロード

```
wget https://openspur.org/~atsushi.w/dataset/mcl_3dl/short_test.bag
```
rviz起動

```
rviz2 -d src/pcl_apps/pcl_apps/config/matching.rviz
```
nodeの起動

```
ros2 launch pcl_apps ndt_matching.launch.py

```
位置姿勢初期値を指定します。

```
ros2 topic pub ndt_matching/initial_pose geometry_msgs/PoseStamped '{header: {stamp: {sec: 1476334344, nanosec: 561867202}, frame_id: "map"}, pose: {position: {x: 0.075144559145, y: -0.0197359323502}, orientation: {z: 0.999897136204, w: 0.0143428383376}}}'
```
rosbag再生

```
ros2 bag play -s rosbag_v2 short_test.bag
```
### 結果
実行すると、以下のようになります。

![Peek 2020-02-06 13-50.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/229952/905090ad-492c-6cf8-3c81-280c021c5265.gif)

~~(ちょっと微妙な気が)~~・・・まあまあですね！
今回はとりあえずここまでとします。