1. saitosasaki

    No comment

    saitosasaki
Changes in body
Source | HTML | Preview
@@ -1,102 +1,102 @@
# はじめに
以前ROS2で3D LiDAR SLAM「lidarslam_ros2」を作ったんですが、だいぶ改良したので各種OSS(Open Source Software)と比較してみました!
以前の記事
ROS2で3D LiDARを用いたGraph SLAMによる三次元地図作成
https://qiita.com/saitosasaki/items/c1ad2e746a92b4808a10
lidarslam_ros2
<a href="https://github.com/rsasaki0109/lidarslam_ros2"><img src="https://github-link-card.s3.ap-northeast-1.amazonaws.com/rsasaki0109/lidarslam_ros2.png" width="460px"></a>
比較の結果
lidarslam_ros2(自分が作ったもの)
![path_tukuba](https://user-images.githubusercontent.com/18298605/82390233-26268380-9a79-11ea-8400-b54bf700b8c9.png)
LeGO-LOAM(KITTIベンチマーク一位のLOAMにグラフSLAMを加えたもの)
![lego-loam1](https://user-images.githubusercontent.com/18298605/82390238-2b83ce00-9a79-11ea-9d8e-e11aa36ced3f.png)
# 比較
データは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の結果です。これはlidarslam_ros2のREADMEのデモを行えば再現できます。
![path_tukuba](https://user-images.githubusercontent.com/18298605/82390233-26268380-9a79-11ea-8400-b54bf700b8c9.png)
緑:パス
格子は尺10m×10mの25×25個の格子です。
![map_tukuba](https://user-images.githubusercontent.com/18298605/82390235-27f04700-9a79-11ea-988e-29099b6a7fd8.png)
メモリに余裕がありましたが、マッピング中にpcd形式でマップを保存する際のCPU使用率が少しきつかったです。
処理もリアルタイムにできています。
## LeGO-LOAM
LeGO-LOAMの結果です。途中からZ方向に大きくドリフトしています。
(LeGO-LOAMは16ラインLiDARではz方向のドリフトが大きく、それに関するissueがよく投げられています)。
https://github.com/RobustFieldAutonomyLab/LeGO-LOAM
![lego-loam1](https://user-images.githubusercontent.com/18298605/82390238-2b83ce00-9a79-11ea-9d8e-e11aa36ced3f.png)
パラメータ・チューニングはしたのですが、いまいち勘所がわかりませんでした。
下記は産総研で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](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/229952/8b37271e-3c2a-b03e-63d1-cce6cbf3d432.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_graph_slamの軌跡をわかりやすくする方法知ってる方がいたら教えてください。rvizやlaunchからでは弄れないっぽい・・・?)、下記の画像のように途中で自己位置推定が失敗しています。
図(丸いのが最新の自己位置です)
![hdl](https://user-images.githubusercontent.com/18298605/82390256-34749f80-9a79-11ea-81a5-bfb82427ed79.png)
グリッド部分を拡大したもの(まだこの段階では自己位置はロストしてません。)
![hdl2](https://user-images.githubusercontent.com/18298605/82390259-35a5cc80-9a79-11ea-99c7-0bcf19f123aa.png)
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を実装する上で得た知見・工夫まとめました。
+以下、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
<a href="https://github.com/rsasaki0109/lidarslam_ros2"><img src="https://github-link-card.s3.ap-northeast-1.amazonaws.com/rsasaki0109/lidarslam_ros2.png" width="460px"></a>
ついでに宣伝ですが、lidarslam_ros2で作った地図で自己位置推定するROS2パッケージも作ったので、良かったら使ってください〜。シンプルな実装ですが、オドメトリもIMUも複合できます。
pcl_localization_ros2
https://github.com/rsasaki0109/pcl_localization_ros2