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

9479 ワード
C++ ROS2 ROS SLAM PCL C++ テキストリンク

はじめに

こんにちは。
本記事はROS2 アドベントカレンダーの12日目です。遅れて申し訳ありません。
前回は@smakitaさんの「WSL に ROS2 をインストールします」、
次回は@youtalkさんの「QoSプロファイルの実践と課題」です。

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

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

マッピング
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データを使います。
ROS2でROS1のbagデータを再生するにはrosbag2に加え、rosbag2_bag_v2をインストールする必要があります。aptで入ります。
ちなみに3D LiDAR SLAMと言ったらvelodyneが有名ですが、velodyneのROS2ドライバーは現状以下の様らしいです。


また、他のドライバーでは以下が見つかりました(動作未確認)

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した結果がこちらです。

大丈夫そうですね！

マップマッチングによる自己位置推定

マップマッチングは、自己位置推定パッケージ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

結果

実行すると、以下のようになります。

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