RTABMAPの論文を読み込むことにした。

前の記事でRTABMAPの使い方を見ていきましたが、中身を深く理解するために論文を読み込むことにしました。

今回読んだ論文

"Long-Term Online Multi-Session Graph-Based SPLAM with Memory Management."

直訳すると
メモリ管理による長期間のオンラインマルチセッショングラフベースSPLAM
これじゃ意味不明ですので、意訳すると
メモリ管理によって長期間、ロボット上（オンライン）で動作するSLAMという感じでしょうか。
英語びっしり約18ページです、ひい。
この論文ではSPLAMという用語が使われていますが、これはSimultaneous Planning, Localization And Mappingの略であり、SLAMに加えてロボットの経路計画まで含んでいます。

アブストラクト

SPLAMにおいては連続的にマップをアップデートしていく必要がありますが、マップが大きくなるにつれ計算速度に制限があるオンボードコンピューターでは計算しきれない大きさになっていきます。そのため、ロボットは自己位置推定とマッピングに用いるマップのサイズを制限しなければなりません。
この論文では、WM(Working Memory)とLTM(Long Term Memory)、やり取りによるメモリ管理を用いて、これらの問題に取り組んでいます。
実験は短距離レーザー、RGB-Dカメラを搭載したロボットで行い、室内で10.5 kmの11回のセッションに分かれています。実験中には139人の人に出くわしました。

イントロ

SPLAMにおいては以下の2つの重要なチャレンジがあります。

マルチセッションマッピング

ロボットの「誘拐問題」、「初期位置問題」ともいわれ、ロボットを起動したとき、ロボットは以前に作られたマップとの位置関係を知らず、目的地までの経路計画を立てることができません。
これに対する一つ目の解決策は、マッピングを始める前に以前作ったマップの中でロボットを自己位置を決めることです。しかしこの方法ではロボットは常に以前のマップの上で起動しなければなりません。
2つ目の方法は起動時に新しくマップを作り、もし、依然来たところに出くわしたとき以前のマップとの位置変換を求める方法です。このプロセスはLoop Closure検出と呼ばれます。
有名なLoop Closure検出手法は画像を用いた手法であり、画像の特徴を抽出して以前の画像と比較します。

動的な環境における長期間マッピング

マップの更新や新しいデータの追加はマップサイズの増加につながります。オンラインSLAMにおいては、データの取得周期よりもマップの更新処理にかかる時間が少なくてはならず、マップが大きくなるにつれLoop closure検出及びマップ最適化の処理にかかる時間が長くなります。

この論文ではこれらの問題に対してメモリ管理機構により対応していきます。

先行研究

略

ロボット

実験に用いたロボットはこれです。一番上にレーザースキャナー、前方にRGBDカメラが付いています。

SPLAMのためのメモリ管理

RTAB-MAPではノードとノード間のリンクにより構成されています。
ノードは以下の情報を持っています。

ID

個別のID

Weight

各ノードは重みをもっています。重要度といってもいいかもしれません。

Bag-of-words

Loop closure検出のための画像のvisual wordです。

センサーデータ

ポーズ；ロボットの位置、方向
RGB画像：visual wordsのために用いられます。
Depth画像：デプス画像はRGB画像とともに登録されます。
レーザースキャン：loop closureの変換やオドメトリの補正、近接リンクの計算に用いられます。

また、リンクには以下の4種類があります。

隣接リンク

ひとつ前と現在のノードの間のリンク

Loop Closureリンク

Loop Closureが検出されたとき追加される新しいノードとマップの中のノードとのリンク

近接リンク

レーザーデータから得られるリンク、近接したノードが

一時リンク

経路計画に用いられる。

RTABMAPのシステム概要

次にRTABMAPの全体像を示します。

基本的にGraph-based SLAMにメモリ管理がくっついた構造です。そこからのマップを経路計画に用いています。

センサーデータを取得し、Working Memory内でLoop closure検出、近接リンク検出を行い、逐次グラフの最適化を行います。
（図左下）

次にメモリ管理ですが、Working Memory内ではマップサイズにより処理時間Tが決まっています。
そのため、必要のないノードはLong Term Memoryに送られ、またLoop Closure検出などで必要な場合Long Term MemoryからWorking Memoryに呼び出されます。そのためどのノードをメモリー内に残すかがメモリ管理のキーとなります。
そのために2つの仮定を置きます。
１、長くいるほどそのエリアが重要であるとみなす。
人が長い時間いた場所ほど記憶に残りやすいように、ロボットが長くいた場所により重みづけをする。
２、ナビゲーションのための重みづけ
ナビゲーションの経路上にある重みも、仮定１によって低く重みづけされることがあるため、仮定２は１より優先されます。

ローカルマップとグローバルマップ。赤いドットはLTMの中のノード、その他はWMの中のノードである。

短期間メモリモジュール

これはセンサーデータがまず初めに集められ、ノードに追加されるポイントです。短期間メモリモジュールの役割はノードの重みを画像の類似度によりアップデートします。
ノードが追加されたとき固有のIDが割り当てられ重みは0に初期化されます。そしてノードには位置、姿勢、RGB画像、深度画像、レーザースキャンが記憶されます。もし、連続した２つのノードが類似した画像を持っているとき（visual wordsの対応の割合が、閾値Yを超えたとき）前のノードの重みに1が加わります。もしロボットが移動していなければ（位置、姿勢が同じ）ノードは追加されません。
また、レーザースキャンを使って、オドメトリの最適化も行われます。レーザースキャンの一致が閾値を超えたなら、近接リンクがアップデートされます。
もしもSTMが一定のサイズを超えたとき、一番古いノードはWMに送られます。

Appearance-based Loop Closure検出モジュール

Loop closure検出はbag of wordsに基づいて、検出されます。事前に設定したLoop Closureのしきい値を超えたときLoop Closureが検出されます。RGB画像はDepth画像とともに登録されすため、visual wordsの3D位置は求められます。Loop Closureが検出されたとき、ノード間の変換が求められます。これはRANSAC(RANdom SAmpling Consensus)により求められます。その後オドメトリと同じようにレーザースキャンのデータにより自己位置を補正します。

近接検出モジュール

Appearance-based Loop Closure検出はセンサーの計測範囲に制限されます。例えば、ロボットがあるエリアを以前訪れたときと反対の方向に向かったとしてもRGBDカメラでは見ている風景が違うためLoop Closureを検出することができません。また、これは特徴の少ない壁や長い廊下などの場合にも発生します。

上：近接検出モジュールなし
下：あり
近接検出モジュールにより、廊下の往復でも適切にマップが作られています。

なお、レーザースキャンは画像ほど特徴的ではないため、対応付けるノードはローカルマップのロボットの周りのノードに限られます。

検索されるノードは半径Rの中のノードに限られ、設定された半径Lの中にあると近接検出を行います。(右上の図a)
なお、Loop Closure検出は近接検出の前に行われます。
そのため、近接検出の前にLoop Closureが検出されると近接検出は無視されます。

グラフ最適化モジュール

グラフ最適化にはTORO(Tree-based netwORk Optimizer)が用いられます。Loop Closure、近接リンクが検出されたとき、オドメトリの誤差がリンクに伝えられ、マップが校正されます。

経路計画モジュール

メモリ管理はどうオンライン経路計画を立てるかに対して、大きな影響を持っています。RTABMAPではMetrical Path Planner(MPP)とTopological Path Planner(TPP)を用いています。

Metrical Path Planning Module

MPPは(x,y,θ)で表わされるポーズ情報を受け取り、ローカルマップを用いて障害物を回避する経路を立てます。DWA(Dynamic Window Approach)により、動的な障害物を回避します。
レーザースキャナーは地面から40cmのところに取り付けられているため、RGBDカメラのデプス情報も障害物検知に用いられます。
地面と障害物を分けるために、（おそらく）点群の平面検出により、平面と比べ設定した角度Zより小さい場所については地面とみなされます。
その後、一定の高さ以上の点については障害物とみなされます。
これにより、ロボットを不整地な環境でも用いることができるようになります。

左：レーザースキャンのみの障害物検知、真ん中：カメラも用いた障害物検知、右：地面のセグメンテーション
左では検知されていなかった椅子の足が、真ん中ではカメラにより検知されている。
右では地面は緑、障害物はオレンジで示されている。

Topological Path Planning Module

TPPがノードのIDで表わされるゴールをユーザーから受け取ったとき、SLAMモジュールによって与えられたグローバルマップを用いて、ゴールまでの経路を計算します。TPPの経路はポーズの連続であり、このステップはオフラインもしくはロボットが動いていないときに行われる必要があります。
TPPはゴールまでのどのノードを選ぶかを決定するため、Dijkstra法を用いています。TPPはローカルマップの最も遠いノードのポーズをMPPに送ります。
そしてローカルマップが更新されるたび、TPPは最も遠いノードをMPPに送ります。

この図の(a)から(f)に向かうにつれだんだん遠いノードが呼び出されて、経路が更新されていきます。これにより、WMの中のノードの数を制限しながら経路計画を立てます。これをゴールにたどり着くまで続けます。

英語の山に埋もれて疲れてしまったので結果は次回書きます。

参考文献

Long-Term Online Multi-Session Graph-Based SPLAM with
Memory Management
https://introlab.3it.usherbrooke.ca/mediawiki-introlab/images/8/87/LabbeAURO2017.pdf