1. saitosasaki

    No comment

    saitosasaki
Changes in body
Source | HTML | Preview

はじめに

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

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

比較の結果
lidarslam_ros2(自分が作ったもの)
path_tukuba
LeGO-LOAM(KITTIベンチマーク一位のLOAMにグラフSLAMを加えたもの)
lego-loam1

比較

データはTier IVが公開しているVelogyne VLP-16のデータを使いました。
このデータでは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の結果です。
path_tukuba
緑:パス
格子は尺10m×10mの25×25個の格子です。
map_tukuba

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

LeGO-LOAM

LeGO-LOAMの結果です。途中からZ方向に大きくドリフトしています。
(LeGO-LOAMは16ラインLiDARではz方向のドリフトが大きく、それに関するissueがよく投げられています)。
https://github.com/RobustFieldAutonomyLab/LeGO-LOAM
lego-loam1
パラメータ・チューニングはしたのですが、いまいち勘所がわかりませんでした。
下記は産総研でSLAMの研究をしている横塚さん(横塚さんはコンピュータビジョンのトップカンファレンスであるCVPRに新しいSLAM手法を通しているツヨツヨ人材です)のロボシン2020での発表ですが、LeGO-LOAMをうまくチューニングできていないことが確認できたので、「横塚さんが無理なら俺も無理」ということでこれ以上深入りしませんでした。

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

ただ、ネットでぐぐってみるとこのような大規模な環境(現在のつくばチャレンジの舞台で全長2-3キロメートル)でもうまく地図を作っている人もいます。そして恐らくこの人は非大学・非企業での参加なので、LiDARは16ライン・・・。
https://twitter.com/embasm/status/1192839129614016512
LeGO-LOAMに詳しい方、アドバイスお願いします・・・。

hdl_graph_slam

hdl_graph_slamの結果です。
https://github.com/koide3/hdl_graph_slam
だいぶ見にくくて申し訳ないのですが(この軌跡をわかりやすくする方法知ってる方がいたら教えてください)、下記の画像のように途中で自己位置推定が失敗しています。
図(丸いのが最新の自己位置です)
hdl
グリッド部分を拡大したもの(まだこの段階では自己位置はロストしてません。)
hdl2
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は地図更新とスキャンマッチングを非同期で処理していません。(なのでndtのsetInputTargetにできあがった点群地図をすべて入れるndt_mappingはどんな強いPCでもすぐにCPUが足りなくなります。autowareで地図を作る場合、ndt_mappingは使わず、ndt_mapping_submapsを使いましょう。あとautowareは地図更新の度にmapをpubishするのも重い原因の一つです。)
lidarslam_ros2ではcppのstdライブラリの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