こんにちは。Abudoriです。 昨年は3DLiDARで3次元地図を作成しました。はじめての3次元地図作成でしたが、GLIMという手法を使用することによって、とても簡単に破綻のない地図を作成することができました。
GLIMは地図作成の機能が提供されているのですが、そのままでは自己位置推定に使用することはできません。ただ、つくばチャレンジシンポジウムで産総研のみなさんに「改造してつかってくださいね〜」と言われているので、使わせていただきたいと思います（実装できればなんだけどな…）。
今回は、まずはGLIMがどのように動作するのかお勉強をします。この機会にSLAMのお勉強をしてしまおうという回です。
GLIMを使用した地図作成方法はコチラ！ www.abudorilab.com
スポンサードリンク
注意
この記事は大きな間違いが含まれている可能性があります。間違いがあってもなくてもコメントで指摘いただけますと幸いです。
大まかな流れ
GLIMを読み解くと、以下のような流れで処理していることがわかります。
GLIMはメインのSLAMの計算を行うglimパッケージ、ROSパッケージとして起動やセンサーの値を送受信したり定期実行のコントロールを行うインタフェースとしてglim_ros2パッケージ、追加でセンサや制約を追加するなどGLIMを拡張するglim_extパッケージで構成されています。glim_extは外部拡張機能なので、ここでは触れません。図中の点線はもし、カメラでVisualOdometryを追加したら、という想定で書き入れています。
大まかに
・センサ情報入力する
・センサデータの前処理をする
・センサデータでオドメトリ推定をする（ここではスキャンマッチで推定）
・オドメトリ推定からスキャンを繋ぎ合わせサブマップを作成
・サブマップの位置の関係性の最適化をして繋ぎ合わせる処理
をすることでSLAMを行います。
今回はglim、glim_ros2パッケージがどのような振る舞いをするのか分解していきます。
glim_ros2
glimとglim_ros2は別のgithubで管理されています。 ROSからGLIMを使用する場合はこのglim_ros2パッケージを起動すると、glimのさまざまな機能を呼び出してくれます。
センサーコールバック
このパッケージが、3DLiDARからの点群を/cloud、IMUからの加速度を/imuのようなトピックをサブスクライブして値を取得します。
点群を受信すると、glim内にある前処理関数を呼び出して、点群を軽くしたり外れ値を除去したりします。 前処理をした点群と直前の点群（厳密にはキーフレーム）と比較して直近のLiDARオドメトリを計算し、格納します。
IMUの加速度を受信すると、タイムスタンプを入れたり、コンフィグのスケールを代入して値を成形します。 LiDARオドメトリのスキャンマッチングの初期値として入力したり、全体最適化のIMUFactorとして入力したりするために、値を更新します。
コードを見てみると、点群やIMUだけでなく、画像もコールバック関数が用意されていることがわかります。 ここでは触れませんが、色つき点群を扱ったり、VisualOdometryをFactorとして追加して、LiDARマッチングが苦手な環境でのロバスト性向上させる改造ができそうですね。
タイマーコールバック
センサーコールバックに対して、こちらはタイマーコールバックです。指定された時間ごとに指定の関数が実行されます。
このように1msごとに、timer_callback()関数が実行されます。
// Start timer
timer = this->create_wall_timer(std::chrono::milliseconds(1), [this]() { timer_callback(); });
ここではあくまでチェックをしているようです。新しいオドメトリ推定があれば追加、新しい全体最適化の要素が増えていれば追加しておき、必要な時に最新の値が参照できるように適宜チェックをしているようです。
glim
こちらでは、GLIMの中核的な処理が書かれています。ROSで実行するときはglim_ros2がインターフェースとして機能しています。 もし、ROS以外のプラットフォームでGLIMを実行したいときは、インターフェースアプリケーションを用意し、glimに処理を投げるような設計をするようにすると良さそうです。いろんな環境で利用されることを想定しているような設計になっていますね。
前処理
ここでは、主に点群の前処理を行っています。 github.com
・ダウンサンプリング
センサーから入力された点群はそのままでは重すぎて現代の計算機では計算が追いつきません。なので、精度が落ちないが低解像度に間引く必要があります。ここでは、単に間引くだけでなく、ダウンサンプリングの方法もマッチングがしやすくなる工夫がされています。
・フィルタリング
ダウンサンプリングされた点群からフィルタリングします。スキャンマッチングのスコア計算に悪影響を及ぼすものを除去します。 おそらくですが、近すぎるか遠すぎるものを除去したり、ロボット自分自身が写ってしまっている点群、周囲の建物や地面のどこにも属さない外れ値の小さな点群をここで除去します。
以上の処理を前処理済み点群のPreprocessedFrameとして返します。
オドメトリ推定
LiDARとIMUの値をつかって短期的なオドメトリ推定を行っています。 ここでは、サブマップの中の限られた範囲でオドメトリ推定を行います。 github.com
・センサデータの読み込み
insert_frame、insert_imuでは、ROS 2のコールバックで得られたセンサデータ（LiDARは前処理済みのフレーム）をpush_backで格納しています。
・オドメトリ推定の実行
オドメトリの推定がrun()によって実施されています。このrun()が非同期で別のスレッドで実施され続けるように作られています。
insert_frame、insert_imuでpush_backしたセンサーデータをすべて使用して、output_estimation_resultsとoutput_marginalized_framesを計算します。
省略しますが、odometry_estimation_base.cppから、odometry_estimation_cpu.cppやodometry_estimation_gpu.cppでスキャンマッチング処理が行われます。そこで得られたスキャンごとの位置の変異を積み重ねた最新のオドメトリ、点群の重ね合わせの蓄積がそれぞれoutput_estimation_resultsとoutput_marginalized_framesになるかと思います。これらをEstimationFrameとして返し、次のサブマッピングに利用するようです。
サブマッピング
オドメトリ推定で作成したEstimationFrameから地図を構成するサブマップを作成します。最終的にはサブマップを繋ぎ合わせることで最終的な地図を構成するため、高品質なサブマップを作成する必要があります。
サブマッピングのメインはこのrun()によって実行されます。 github.com
insert_frame()、insert_imu()によってEstimationFrameから情報を取得します。
フレームが追加されるたびに実質的にサブマップをマッピングしていきます。
github.com
・オドメトリ推定での姿勢の移動を拘束の一つとする
・IMUが有効なときは、IMUを積分した移動を拘束の一つとする
・そのほか、画像の移動量などが追加されていたら拘束の一つとする
・これらの拘束条件を満たすグラフ構造を作り、もっとも破綻のないグラフ構造にする
・キーフレームから一定距離が離れるまでこれを繰り返す
・キーフレームから一定距離以上離れたら、最も誤差の少ない状態の位置関係の座標でスキャンを繋ぎ合わせる
・サブマップとして保存する
大まかにはこのような動作をしてサブマップを構築しているように見えます。 それぞれの拘束条件をファクタと呼び、このファクタ同士がどのように影響をしあうか（前後のスキャン同士やIMUの移動量）を最適化します。実装では、GTSAMを使ってこれらの誤差を最小化する処理を行います。
グローバルマッピング
前述しましたが、グローバルマッピングはサブマッピングと似た処理をします。サブマッピングでは、直前のキーフレームとなるスキャンからのファクタを最適化していましたが、グローバルマッピングでは、サブマップ同士の関係性をファクタとして扱い、ファクタグラフを構築し最適化しています。
サブマップが増えていくごとにそれぞれのサブマップ同士にどのような関係性があるかをファクタグラフに追加していきます。 github.com
サブマップ同士のオーバーラップを見つけてファクタを作成しています。これはいわゆるループクローズとして働きます。 github.com
前後のサブマップであれば、IMUの動きの積分値でも関係性を定義することができます。 github.com
ファクタグラフを最適化することでサブマップ同士の位置がより破綻のない関係性がある位置に置き換わります。 単純にスキャンマッチングを発見し位置を定義するのではなく、スキャンマッチングやIMUの動き、またスムージング処理による拘束や、画像やGNSSなど外部実装によってついかされた拘束条件などあらゆる条件のなかで誤差が最小化されたサブマップの位置関係を構築します。こうした位置関係からサブマップの点群を繋ぎ合わせることで、巨大な地図でも破綻のない点群を構築することができます。 github.com
また、このファクタグラフ最適化は、ループクローズなど新しい発見が起きたとき、”すべての地図”を更新できます。 近年のグラフSLAMでは、ループクローズが成立した地点から、逆算的に誤差を修正していきますが、ファクタグラフはいままですべての地図の相対位置関係が記録されているものを更新していくため、ループクローズが起きたところとは一見関係ないところも誤差が最小化されれば更新されるようです。この振る舞いでよりロバストに破綻のない地図を構築することに貢献しています。これを実現できるのもGPUを巧みに活用できるからでしょう。
スポンサードリンク
GLIIMを応用しよう
今回はZEPエンジニアリングの講習を受けてきた1次アウトプットとして”コードを読んでみる”というのをやってみました。 GLIMを有効活用するためにも少しずつ理解度を上げていきたいところです。シンプルですが、GLIM改造のまず第一歩としてコードを解釈してきました。間違っている理解も混じっているかと思いますので、指摘してください。
間に合えば、つくばチャレンジ2025にSLAM以外の機能を持たせることができたらいいな、と思います。予定は未定です。
