RTABMAPをよむ前篇

こんにちはSENSYN ROBOTICS（センシンロボティクス） Advent Calendar 2019の 16日目担当の @ohtt です。

　SLAMに関して調査をしており、rtabmap_rosを試しに使用してみようとインストールしたもののプロジェクトが巨大でオプションも大量にあり理解していくためにソースコードから読んでいくのはなかなか難しそうでした。なので方向性を変えて、ドキュメントからRTABMAPを理解していこうということで、RTABMAPを解説している文書を読むことにしました。

　この記事は、理解した内容を要約して記述していきますが、勝手に書き飛ばしていく部分もあります。欠落する部分などもありますが、どなたかの役に立てば幸いです。

読んだ文章

M. Labbé and F. Michaud, "RTAB-Map as an Open-Source Lidar and Visual SLAM Library for Large-Scale and Long-Term Online Operation,” in Journal of Field Robotics, vol. 36, no. 2, pp. 416–446, 2019.

目次
- Abstract
- 1 Introduction
- 2 Popular SLAM Approaches Available on ROS
- 3 RTAB-Map Description
- 4 Evaluating Trajectory Performance of RTAB-Map Using Different Sensor Configurations
- 5 Evaluating Computation Performance between Visual and Lidar SLAM Configurations with RTAB-Map
- 6 Discussion
- 7 Conclusion

本記事では2のPopular SLAM Approaches Available on ROSまでを書いていくこととします。RTABMAPの開発歴史などから、なぜこんなにも巨大なプロジェクトになったのかが分かるかと思います。

Abstract

　元々RTAB-Mapは、規模の大きなMAPかつ長時間での運用のためににメモリ管理機能を持たせたappearance-basedなループクロージャ検出手法としてスタートしました。

SLAMもRTABMAPプロジェクトに含まれるようになりましたが、センサー、処理能力、精度、計算能力、統合の容易さの観点から、どのSLAMアプローチを使用するのが最も適切かという問題が生じます。この問題に対処するため、VisualBase,LidarBaseさらに2D,3Dの様々なSLAM手法に対応し比較検討できるようにしました。
そうすることで、1つのパッケージで、ユーザーがさまざまなロボットおよびセンサーを使用して幅広いアプリケーション向けのさまざまな3D、2DのSLAMソリューションを実装および比較できるツールとなりました。

このホワイトペーパーでは、現在のRTAB-Mapを自律ナビゲーションアプリケーションの実用的な観点から、VisuaSLAMlおよびLidarSLAMにおける構成の長所と限界を概説していきます。

1 Introduction

RTABMAPは制限時間内にループクロージャを実行するために地図のサイズを制限しています。それによって長時間、規模の大きな環境でのマッピングを可能とします。
2009年から始まり、様々な環境やアプリケーションに対応するためグラフベースSLAMを採用、最終的にスタンドアローンのC++Library、ROSPackageへと変化していきました。これらの変更は以下に示すような要求から求められるものでした。

Online processing:
SLAMからの出力はセンサーデータを受けっとた後一定の時間内に出力されなくてはない。
しかし特にグラフベーススラムではMAPが大きくなると、それだけグラフ最適化やMAPの構築のための処理に時間がかかる。
Robust and low-drift odometry
未知の環境や特徴量が十分無い環境ではSLAMの自己位置推定がうまく行かない場合もある。なのでそのような場合はOdometoryが重要となる。Odometoryをロバストに行うためにIMU,ホイールエンコーダ、LidarまたはCameraの情報を混ぜて推定を行う。
Robust localization:
さまざまな環境変化はSLAMが自己位置推定できなくなる要因になるので、SLAM自体がロバストでなくてはならない。
Practical map generation and exploitation:
多くのnavigationはocupancy grid map ベースのアプローチをとっている。また2D、3Dのoccupancy grid map を提供するSLAMを開発すればシステムインテグレーションが容易になるという利点がある。
Multi-session mapping
システムを起動した直後、ロボットは前回作った地図との相対的な自己位置がわからない。これでは前回のMapping中に訪れた地点へのPathPlanninngを行うことはできない。ロボットがmapping をゼロから再施行するのを避けるため、multi settion mapping 機能は、システムの開始時に新しい地図を作り、以前訪れた地点に到達したら２つの地図の間の位置変換を行う。
この機能により環境の地図全体を再度Mappingする必要性はなくなる。

様々なSLAM手法が存在するがそれらを比較するのは非常に困難であった。そのためRtabmapは様々なSLAM手法を比較検討できるように発展していった。Section2では現在ROSで使用可能なSLAM手法を紹介していきます。

2 Popular SLAM Approaches Available on ROS

ROSで使えるOpenSourceのSLAMは様々です。
まずはLidarBaseのアプローチの紹介から始めます。

GMapping [Grisetti et al., 2007] TinySLAM [Steux and El Hamzaoui, 2010]
ロボットの軌道を推定するためにパーティクルフィルタを使っています。
Hector SLAM [Kohlbrecher et al., 2011]
2DLidarから2DのOccupancyGridMapsを低い計算リソースで高速に作成できます。
IMUなどの外部センサーを使用して、ロボットの位置を3Dで推定することもできます。一方で、ループクロージャーを検出しないため、マップの補正・修正ができません。
また、Hector SLAMは外部オドメトリを必要としません。ロボットからOdometoryが得られない場合には利点となりますが、レーザーキャンのマッチングがうまく働かないような、幾何的な制約が多くない環境で操作する場合は不利となることもあります。
ETHZASL-ICP-Mapper,based on libpointmatcher library [Pomerleau et al., 2013]
2D OccupancyGridMapsを2DLidarから作成するために使います。加えて、組み合わせた点群を2Dまたは3DLidarから作成するために使用できます。
ただし、Hector SLAMと同様にループクロージャの検出を行わないため、マップの誤差は修正できません。
Karto SLAM [Vincent et al., 2010], Lago SLAM5 [Carlone et al., 2012] and Google Cartographer [Hess et al., 2016]
　Lidar graph-based SLAMです。グラフ表現から2D OccupancyGridMapを生成します。
Google Cartographerは、3D Lidarをサポートしており、3D点群出力もできるので、バックパックマッピングプラットフォームとしても使用できます。
マッピング中、グラフ内の制約によって紐付けられたサブマップが作成されます。ループクロージャが検出されると、サブマップの位置が再度最適化され、センサーのノイズとスキャンマッチングのによる誤差を修正します。
Hector SLAMとは異なり、外部Odometoryを使えば、幾何的な複雑度が低い環境でも、よりロバストなスキャンマッチングによる自己位置推定を実現できます。
BLAM
　Lidar graph-based SLAMです。環境を3D点群として生成するために3D Lidarのみをサポート。ドキュメントによれば、ロボットが以前訪れたことがある位置に来たときに、スキャンマッチングによってローカルなループクロージャーが動作。GTSAMを使用してマップを最適化するようです。
これはBLAMがグローバルなMapの最適化ができないということを意味しています。
SegMatch [Dub ́e et al., 2016]
　3D Lidar graph-based SLAMとしても使用できる3D Lidarベースのループクローズ検出アプローチです。ループクロージャは、ポイントクラウドから作成された3Dセグメント（車両、建物、樹木の一部など）を一致させることで検出されます。

これらのLIDARベースのSLAMアプローチでは、SegMatchのみがマルチセッションまたはマルチロボットのマッピングに使用できます。

VisulaSLAMに関して、多くのオープンソースの手法が存在しますが、ロボットで簡単に使用できるものは多くはありません。マッピング中に環境の実際のスケールを推定する必要性があるからです。そのような問題に対処できるアプローチにレビューを限定するので、次で紹介するVisualSLAMアプローチ（たとえば、ステレオおよびRGB-Dカメラまたは視覚慣性オドメトリーを使用）は、スケールドリフトの影響を受けません。

maplab [Schneider et al., 2018] and VINS-Mono [Yi et al., 2017]
visual inertial graph-based SLAMです。IMUとカメラのみを使用して、自己位置推定用のビジュアルマップを提供します。
maplabは2つのステップで実行します。まずデータを記録します。そしてオフラインでループクロージャ検出、グラフ最適化、マルチセッション、密マップ再構築を行います。その後、作成できたMapをLocalizationModeで使用します。
一方VINS-Monoの地図作成はオンラインで行われます。GPUで計算したローカルTSDFボリュームマップを用いて、障害物回避と経路計画を行うことができます。とても広い環境での処理時間を制限するために、VINS-Monoはグラフのサイズを制限し、ループクロージャのないノードを最初に削除してから、グラフの密度に応じて他のノードを削除しています。
ORB-SLAM2 [Mur-Artal and Tard ́os, 2017], S-PTAM [Pire et al., 2017]
現在ステレオカメラで使用できるstate-of-the-artな、feature-based visual SLAMです。
ORB-SLAM2の場合、RGB-Dカメラでも使用できます。
ORB-SLAM、S-PTAM、はgraph-based なSLAM アプローチです。DBoW2を使用してループの閉鎖が検出されると[Ǵalvez-Ĺopezand Tard́os、2012]、バンドル調整によりマップが最適化されます。
ループクローズ後のグラフ最適化は、カメラトラッキングフレームレートのパフォーマンスに影響を与えないように、別のスレッドで実行されます。
DVO-SLAM [Kerl et al., 2013], RGBiD-SLAM [Gutierrez-Gomez et al., 2016] and MPR [Della Corte et al., 2017]
モーションの推定のためにローカルなVusual特徴量を使わずに、RGB-D画像全体における光度と距離の誤差を使う。環境の密な点群を生成できます。
MPRはLidarと併用することもできますが、これはOdometoryを計算するためだけに使われます。
DVO-SLAMには、位置推定とは独立したループクローズ検出アプローチがないため、規模の大きなマッピングにはあまり適していません。
ElasticFusion [Whelan et al., 2016], Kintinuous [Whelan et al., 2015], BundleFusion [Dai et al., 2017] and InfiniTAM [K ̈ahler et al., 2016]
truncated signed distance field（TSDF）ベースなアプローチです。オンラインでサーフェスベースなMapを作成できますが、パワフルなGPUが必要です。
ElasticFusionの場合、小さな環境ではリアルタイムでカメラフレームを処理できますが、マップ内のサーフェスの数に応じてフレームあたりの処理時間が増加します。
BundleFusionの場合、環境のサイズに応じて、ループクロージャ検出の際にグローバルなglobal dense optimization timeが増加します。
InfiniTAMは高速にループクロージャ動作するように見えますが、やはりループクロージャの検出と修正のための処理時間は環境のサイズに応じて増加します。
これらのアルゴリズムはオープンソースでありながら、ROSをサポートしていません。これは、GPU上で実行されるマッピングととラッキングが非常に高速で密に結合、依存しているためです。

ここまでで紹介したVisualSLAMはすべて、カメラが遮られたりせず、画像中に常にトラッキングに十分な特徴量があることを前提としています。
白い壁のような特徴量に乏しい環境がなかったり、人や障害物に遮られたりしないというような仮定を自律移動ロボットが動作する環境が常に満たせるとは限りません。

次に紹介するVisualSLAMアプローチは、このような状況に対してよりロバストになるように設計されています。

MCPTAM [Harmat et al., 2015]
複数のカメラを使用して、システムの視野を拡張します。少なくとも1台のカメラがVisualな特徴量を認識できていれば、MCPTAMは位置を見失いません。
RGBDSLAMv2 [Endres et al., 2014]
外部からOdometoryを与えて自己位置推定行えます。robot_localization のような拡張カルマンフィルタをつかって複数のセンサー情報をフュージョンさせるROS packageを使えばよりロバストなOdometoryを得ることができます。
RGBDSLAMv2は、3D OccupancyGridMap（OctoMap [Hornung et al。、2013]）および密な点群環境を生成できます。

RTABMAPをよむ 前篇

読んだ文章

Abstract

1 Introduction

2 Popular SLAM Approaches Available on ROS

Summary Table

RTABMAPをよむ前篇