はじめに

以前ROS2で3D LiDAR SLAM「lidarslam_ros2」を作ったんですが、だいぶ改良したので各種OSS(Open Source Software)と比較してみました！

以前の記事
ROS2で3D LiDARを用いたGraph SLAMによる三次元地図作成
https://qiita.com/saitosasaki/items/c1ad2e746a92b4808a10
lidarslam_ros2のGithubレポジトリ

比較の結果
lidarslam_ros2(自分が作ったもの)

LeGO-LOAM(KITTIベンチマーク一位のLOAMにグラフSLAMを加えたもの)

比較

データはTier IV社が公開しているVelodyne VLP-16のデータを使いました。16ラインLiDARは3D LiDARで一番お手頃なものなので、それらをターゲットにlidarslam_ros2を開発しました。
このデータではLiDARが2kmのコースを歩行程度の速さで約40分間移動します。
https://data.tier4.jp/rosbag_details/?id=212。

今回は、以下の3つで比較しました。
・lidarslam_ros2(自分が作ったもの)
・LeGO-LOAM(KITTIベンチマーク一位のLOAMにグラフSLAMを加えたもの)
・hdl_graph_slam(ndt/gicp SLAMで人気なもの)

テストした自分のPCのスペック

メインメモリ：16GB
クロック速度：3.5GHz
キャッシュメモリ：8MB
コア：4コア

lidarslam_ros2

私が作ったlidarslam_ros2の結果です。これはlidarslam_ros2のREADMEのデモを行えば再現できます。

緑：パス
格子は尺10m×10mの25×25個の格子です。

マッピング中、メモリには余裕がありましたが、pcd形式でマップを保存する際のCPU使用率が少しきつかったです。
処理もリアルタイムにできています。

LeGO-LOAM

LeGO-LOAMの結果です。途中からZ方向に大きくドリフトしています。
(LeGO-LOAMは16ラインLiDARではz方向のドリフトが大きく、それに関するissueがよく投げられています)。
https://github.com/RobustFieldAutonomyLab/LeGO-LOAM

パラメータ・チューニングはしたのですが、いまいち勘所がわかりませんでした。
下記は産総研でSLAMの研究をしている横塚さん(横塚さんはコンピュータビジョンのトップカンファレンスであるCVPRに新しいSLAM手法を通している人です）のロボティクス・シンポジア2020での発表ですが、LeGO-LOAMをうまくチューニングできていないことが確認できたので、「横塚さんが無理なら俺も無理」ということでこれ以上深入りしませんでした。

LiTAMIN: LiDAR-based Tracking And MappINg
https://www.youtube.com/watch?v=ydnM3xWcgyA&feature=youtu.be&app=desktop

ただ、ネットでぐぐってみるとこのような大規模な環境(現在のつくばチャレンジの舞台で全長2-3キロメートル)でもうまく地図を作っている人もいます。そして恐らくこの人は非大学・非企業での参加なので、LiDARは16ラインか2D LiDARを複数台で回転させていると思われます。凄い…

LeGO-LOAMすごい．（高低差があるため，2.5Dの現行位置推定ロジックでは逆にすぐに使えないのが課題） pic.twitter.com/OzSZuJrP2C

— yone (@embasm) November 8, 2019

LeGO-LOAMに詳しい方、アドバイスお願いします・・・(おそらくIMU/オドメトリをうまく使っている？）。

hdl_graph_slam

hdl_graph_slamの結果です。
https://github.com/koide3/hdl_graph_slam
だいぶ見にくくて申し訳ないのですが(このhdl_graph_slamの軌跡をわかりやすくする方法知ってる方がいたら教えてください。rvizやlaunchからでは弄れないっぽい・・・？）、下記の画像のように途中で自己位置推定が失敗しています。
図(丸いのが最新の自己位置です)

グリッド部分を拡大したもの（まだこの段階では自己位置はロストしてません。)

hdl_graph_slamのissuesを見てもわかるように、16行のLIDARではhdl_graph_slamはうまく動作しません。

Cartographrer

今回、Cartographrerは私の貧弱なPCのスペックでは動かせない（たいていみんなワークステーション等の強いPCで動かしています。また、処理時間も計測時間の数倍以上かかります。）ですし、3DではIMUが必要なので使っていません。
https://github.com/cartographer-project/cartographer_ros。

lidarslam_ros2で工夫したところ

以下、lidarslam_ros2を実装する上で得た知見・工夫をまとめました。

地図更新は非同期で処理

hdl_graph_slamやautoware ndt_mappingは地図更新とスキャンマッチングを非同期で処理していません（地図更新中はスキャンマッチングができない）。そうすると、リアルタイムでスキャンマッチングが間に合わなくなったりします。
lidarslam_ros2ではcppの標準ライブラリであるthreadとfutureを使って地図更新をスキャンマッチングと非同期で処理しています。

Zドリフトへの対処

Inaccuracies in tracking Z for drone SLAM #83
https://github.com/koide3/hdl_graph_slam/issues/83

上のhdl_graph_slamのissueによると、「16ラインのLiDARだとNDTは良くない、GICPを使おう」
とあるんですが、よりクリティカルな問題はNDT以前にhdl_graph_slamがmap2pointではなくpoint2pointだからだと思います。実際lidarslam_ros2をmap2point（正確には単一スキャンのみではなく蓄積されたスキャン群をtargetに使う)にしたら16ラインでもドリフトはだいぶ抑えられました。

あと、Zドリフトに関しては、6軸IMUで推定したroll,pitchをそのままOSS(LeGO-LOAMとか)に入れるといい感じにマッピングしてくれます。他にはオドメトリやIMUで点群の歪み補正や動的障害物の排除等の前処理も有効です。

おわりに

比較してみて自分の貧弱なPCで、16ラインのLIDARのみでの実験である場合は、他のOSSと比べて良い結果になりました。

ROS2でSLAMを書いていてひしひしと良いと思ったのは、よく謎バグに煩わされたROS1のnodeletを書かなくて良かったことですね!ROS1で消耗しているならROS2に移行するのも一つの手だと実感しました。

ともかく、頑張って作ったのでぜひ使ってみてください〜。
ということでもう一度宣伝です。
Github
lidarslam_ros2

ついでに宣伝ですが、lidarslam_ros2で作った地図で自己位置推定するROS2パッケージも作ったので、良かったら使ってください〜。シンプルな実装ですが、オドメトリもIMUも複合できます。
pcl_localization_ros2
https://github.com/rsasaki0109/pcl_localization_ros2