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

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

書いたコードはGithubにあります。
https://github.com/rsasaki0109/graphslam_ros2

ループクローズにより修正された経路

地図

事前知識

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

スキャンマッチングに関しては以下のスライドを読めばわかると思います。
Scan Matching 自己位置推定手法
LiDAR-SLAM チュートリアル資料

NDT scan matchingについて詳しくは以下を参照ください。(GICPに関して、解説してる日本語記事はわかりませんが、元論文が比較的簡単に読めると思います。）
gggggraziegrazie NDT( Normal Distributions Transform )
NDTスキャンマッチング 第1回3D勉強会@PFN 2018年5月27日

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による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にある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 

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

Green: path with loopclosure, Yellow: path without loopclosure

地図

さいごに

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