オリジナルセンサデータでSLAM！GLIMで美しい3DMAPを作る【つくチャレ2024-4】

ランキング参加中

ロボット

Abudoriです。前回の記事では、GLIMでSLAMするために必要なセンサの準備をしました。今回の記事では、実際に撮ったセンサデータを実際にGLIMに読み込ませて巨大な3DMAPを作っていきます。

前回の記事の準備で実際にセンサを持って街を歩きました。1時間くらい計測を続けて以下のような巨大で綺麗なMAPを作ることができました。

前回のセンサデータを撮りにいくまでの記事はこちら www.abudorilab.com

GLIM自体のインストール方法はこちら www.abudorilab.com

今回のブログの内容を動画にまとめました。まずはこちらをご覧ください。ブログ記事はコマンドの打ち方などの詳細を紹介しております。 www.youtube.com

スポンサードリンク

rosbagをGLIMに読み込ませよう

GLIMのインストールとrosbagがうまく撮れていれば、GLIMを起動した後にrosbagを再生すれば、自動的にSLAMが開始されます。

config値を調整しよう

configファイルはglim_rosのパッケージ構成内にあります。

ソースビルドした人は

colcon_ws/src/glim_ros/config

にconfigファイルがあります。

colcon_ws/src内にあるファイルはcolcon buildの際にビルドされ、実行ファイルがinstall/shareにコピーされます。このshare内に各パッケージのビルド済み実行ファイルがあり、これをros2 runで実行しているわけです。つまり、colcon_ws/src/glim/configを変更した後には、必ずcolcon buildを実行してください。

apt installした人は

/opt/ros/jazzy(自分のROSバージョン)/install/share/glim_ros/config

にconfigファイルがあります。 /opt内は一般ユーザには書き込み権限がありませんので、sudo権限を与えてファイルを編集してください。

sudo gedit hoge_config.json

まずは、GLIMに読み込んで欲しいtopic名を記載します。config_ros.jsonを変更します。一番下にある// Topicsの項目を変更します。 github.com

今回はvelodyneとwitmotionを利用しているので以下のようにしました。ご自身の環境に合わせて変更してください。

    "imu_topic": "/imu",
    "points_topic": "/velodyne_points",
    "image_topic": "/image"

カメラシーケンスを使用すると、よりロバストになるらしいのですが、使用しない場合はこのままで良いと説明があったためこのままにします。

TFの設定ですが、特に変更をしませんでした。 github.com 今回はVelodyneとIMUの2つしかつけていないセンサのみのロボットという扱いです。imu_frameはたまたまあっていましたが、lidar_frameは"velodyne"でした。ここもご自身の環境に合わせて変更します。

    "imu_frame_id": "imu",
    "lidar_frame_id": "velodyne",

回転式LiDARで特に16本のものを使用している場合は以下のパラメータも必ず変更します。

使用しているセンサや測定した環境によってはうまく動作しないことはあります。というか、初期値で動くことはあまりないと思います。(思ったより動いたのでびっくりしましたが！) なので、SLAMをするうえでの設定値を設定して何度か試してみてください。

このページを参考にconfigを煮詰めてください（IMUを使用せず、LiDARベースのみで使用する設定もここに例があります）。 github.com

試しに一度GLIMを試してみる

GLIMを起動します。(コマンド未確認)新しい端末を開いて以下のコマンドを入力します。

ros2 run glim_ros glim_rosnode

次のような画面が表示されている状態にします。

次にrosbagを再生します。 GPU実装版をインストールできている場合、非常に高速に実行ができるので、4倍速程度であれば安定動作しました。前回の記事で撮ってきたrosbagをGLIMを実行するPCにコピーし、ファイルの場所を覚えておきます。

rosbag を再生するには以下のコマンドを入力します。

ros2 bag play -r 4 your_rosbag_directry

-r (rate)オプションでは倍速再生する速度を指定できます。今回は4倍速を指定しました。実際にやってみて速度を調整してください。CPUモードで非力なパソコンを使用している場合は逆に遅くすることもできます。-r 0.5と指定すると倍の時間をかけてrosbagを再生します。

ここでうまくいった方はおめでとうございます。次の節に進んでください。

うまくいかなかった場合はconfigを見直してみたり、環境を変えてみましょう。環境は小さな公園が初めてのテストにはちょうど良いです。 6畳の自室では空間が小さすぎてLoopCloseが起こらず地図構築ができず、大きすぎる環境では何が原因でうまくいかないのか初心者には判定が難しいです。

スポンサードリンク

地図を出力しよう

rosbagを最後までプログラムが終了せずに再生でき、ぱっと見いい感じに地図構築ができたら最終調整をしましょう。 GLIMは後からLoopCloseの追加やマッチングを手動指定して再構成することができます。これが最高に便利です（それまではパラメータを微調整して長いセンサデータを何度も再生しなおすリセットマラソンが必須でした）。

最後までrosbagが再生できたら、Ctrl + cでプログラムを終了してください。この時に自動的にSLAMした構成データが以下の場所に保存されます。

/tmp/dump

Ctrl+Cかウィンドウのバツボタンで自動保存される。右上の端末にダンプデータが保存された場所が書かれている。

このdumpディレクトリを読み込ませるわけですが、このままでは読み込めない、上書きされてしまうので好きな場所に移動しましょう。

mv /tmp/dump ~/sukina_basyo/tmp_map_data

次に手動マッピングエディタを起動します。以下のコマンドを入力します。

ros2 run glim_ros offline_viewer

すると以下のような画面のプログラムが起動します。左上のタブから、File->readを選択、先ほどの移動した構成データを指定します。

このエディタでは、基本的に同じ場所にある場所のサブマップ同士を指定してくっつかせる処理を指定する作業をします。本来、この処理はSLAMの最中に行われますが、マッチングが起きない、誤マッチングしてしまった、ことで地図がずれてしまうことがあります。これを人間があとから正してあげて、リアルな地形に近づけるようにします。

ここをこう繋げる！の図

スタート地点がズレてしまっている。スタート地点とゴール地点は全く同じ場所なのでこのズレた点群同士をくっつける。

近年の多くのSLAMはLiDARの数スキャン分のデータを合わせて周辺の小さな地図（以下、サブマップと呼びます）を作成し、このサブマップ同士をなるべくうまく繋ぎ合わせることで全体の大きな地図を構成しようとします。SLAMの概要はこちらの記事も合わせてご覧ください。 www.abudorilab.com

サブマップ同士をどんなにうまく繋いでも少しずつズレていきます（GLIMはここにも大きな工夫点があるようです）。このズレはしょうがないものとして扱い、後で同じ場所に戻ってきた時に合わせてつじつまを合わせる処理をLoopCloseと言います。LoopCloseの概要はこちらも合わせてご覧ください。 www.abudorilab.com

このツールでは、以下の写真のようにサブマップがわかりやすく可視化されます。このサブマップを人間の目でチェックして再構成を指示することができます。先ほどの1周2.8kmの歩行データをSLAMすると最後の地点がさすがにズレてきます（それでも10〜20m程度でとてもと〜〜っても少ないです）。ここを合わせてみましょう。

合わせるポイントを指定します。センサデータを記録する時にスタート地点をベンチの真ん中にし、帰ってきた時に同じ場所にセンサを置いて計測を終えました。つまり、最初と最後のサブマップは現実世界ではほとんど同じ地形になっているはずです。この強い仮定をSLAM結果に反映させます。

近くのサブマップ同士は互いに連結されているので、最後の位置がどのくらいズレていたのかという情報が得られたら、遡って反映していき、全体の誤差が最小になるように再調整をします（実装によっては、この説明は異なるので詳細は現論文をご参照ください）。こうすることで最終的に出力される全体の地図が整合の撮れたキレイな地図になるわけです。

現論文はこちら arxiv.org

最後にFile->saveからファイル名を指定して保存することができます。これで3Dデータが手に入ります。

あとは煮るなり焼くなりしてロボットを走行させましょう！！！

スポンサードリンク