TUMのRGB-D SLAMについてまとめる

# [Computer Vision Group](http://vision.in.tum.de/)
TUM（ミュンヘン工科大学）のコンピュータビジョングループは、SLAM（スラム）の研究がすごい。SLAM(Simultaneous Localization and Mapping)：自己位置推定と地図作成を同時に行うことであり、UAVsやAR, VRの分野で応用される。[KinectFusion](http://qiita.com/SatoshiRobatoFujimoto/items/bddefc89456c0716b00a)なんかもそう。このページでは、[RGB-D SLAM](http://vision.in.tum.de/data/datasets/rgbd-dataset)についてメモしていく。

# [Real-time 3D visual SLAM with a hand-held camera, European Robotics Forum, 2011.](http://vision.in.tum.de/publications#2011)

これが最初のRGB-D SLAMである。

## アルゴリズム
アルゴリズムは、以下のようになっている。
1. RGB-D画像の読み込み
2. SURF特徴量抽出とマッチング
3. 自己位置推定（RANSAC）
4. 自己位置の補正（ICP）
5. ポーズグラフによる最適化（HOGMAN）
6. 3次元モデルの出力（色付き）

## プログラム
オープンソースでROSに[公開](http://wiki.ros.org/openni/Contests/ROS%203D/RGBD-6D-SLAM)してある。

論文では、Kinectをゆっくりと動かし撮影された12枚のRGB-D画像を用いている。モデルの出力速度は0.5fpsである。（Intel i7 with 2GHz）
しかし、ground truth（真値）がなく、このモデルの評価ができない。

# [Towards a benchmark for RGB-D SLAM evaluation, RSS'11](http://vision.in.tum.de/publications#2011)

ground truth(真値)がないため、RGB-D SLAMを評価するためにデータセットとベンチマークを作成。真値は、高速なモーションキャプチャシステム（from MotionAnalysis）により得られる。

## RGB-Dデータセットとベンチマーク
データセットの作成方法は以下のようになっている。
1. 30HzでRGB-D画像の取得（OpenNIドライバ）
2. 500HzでKinectの加速度センサを記録
3. カメラの内部パラメータは出荷時のものに設定
4. カメラの軌道を100Hzで取得（モーションキャプチャシステム）
5. モーションキャプチャシステムとKinectの座標変換（キャリブレーション）
6. 外部からカメラの軌道と実験環境の動画を取る

このようにして作成された50GBのKinect[データセット](http://vision.in.tum.de/data/datasets/rgbd-dataset)が公開されている。(クリエイティブ・コモンズ・ライセンス)

これによって、絶対軌道誤差（ATE）や相対位置誤差（RPE）が測れる。
ATEはVisual SLAMの評価に、RPEはVisual Odometryのドリフトの評価に用いる。

# [Real-Time Visual Odometry from Dense RGB-D Images, ICCV'11](http://vision.in.tum.de/publications#2011)

visual odometryの推定にenergy-based approach（のちにdense visual odometry, Direct Methods）を用いる。
今までは、SIFTやSURFなどの特徴量を用いていたため、Feature-based Methodsと呼ばれる。

## キーアルゴリズム
Maximizing photo-consistency
バックプロジェクションエラー(輝度差)の最適化によって、次のフレームを最初のフレームに位置合わせする変換行列を求める問題。

## 結果
RGB-Dデータセットを用いて、[GICP](http://www.roboticsproceedings.org/rss05/p21.pdf)（Generalized-ICP）と比較、評価する。

| Dataset | Ours | GICP | Improvement |
|:-----------:|:------------:|:------------:|:------------:|
| freiburg1/desk | 0.0053m | 0.0103m | 1.94x |
| | 0.0065deg | 0.0154deg | 2.37x | 
| freiburg2/desk | 0.0015m | 0.0062m | 4.13x |
| | 0.0027deg | 0.0060deg | 2.22x |
各フレームごとのドリフト（RPE）の中央値[mm/frame]を表している。

GICPでは、すべてのフレームの15.3%に1cm以上の誤差が出ており、提案手法では2070フレーム中(freiburg2/desk)1フレームだけであった。
また、実行速度については、Intel Xeon E5520 CPU with 2.27GHzで提案手法が0.08s(real-time at 12.5Hz), GICPは7.52s per matchである。


# [An Evaluation of the RGB-D SLAM System, ICRA'12](http://vision.in.tum.de/publications#2012)
RGB-Dデータセットとベンチマークを用いたRGB-D SLAMの評価をしている。

- データセットのfr1における平均精度と処理時間の結果
- SIFTGPU、SURF、ORBにおけるVisual Odometryの評価
- SIFTGPU、SURF、ORBにおける実行速度の評価
- マッチングの際に用いるFLANNとBrute Force, 2種類のNN(nearest neighbor method)の処理速度の評価

## アルゴリズム

1. RGB-D画像の取得
2. RGB画像から特徴点マッチング（SURF,SIFT,ORB）
3. マッチング結果より、変換行列推定（RANSAC,GICP）
4. ポーズグラフによる最適化（g2o）
5. 3次元点群の位置合わせ
6. OctoMapによるボクセル化
7. 3次元占有グリッドマップを出力

## 結果
提案手法（SIFTGPU and FLANN matching）は、精度（RPE.RMSE）が9.7cm、3.95°、処理時間は0.35s/frame。
SIFTGPU/SURFの精度がよく、ORBが最も高速。
SIFTGPUがSURFに比べて3.5倍速い。
マッチングはFLANNが速い。
このRGB-D SLAM SYSTEMはオープンソースで[ROS](http://wiki.ros.org/rgbdslam/)に公開してある。[（評価）](http://wiki.ros.org/rgbdslam_electric/evaluation)

# [A Benchmark for the Evaluation of RGB-D SLAM Systems, IROS'12](http://vision.in.tum.de/publications#2012)

RGB-Dデータセットとベンチマークについて記述してある。データセットを用いる場合は、この論文を参考文献に入れること。

## データセットの作成方法
- モーションキャプチャシステムのキャリブレーション
- Kinectのキャリブレーション
- 外部パラメータのキャリブレーション
- キャプチャ時間の同期

## 評価の仕方
- Relative Pose Error(RPE)の測定方法
- Absolute Trajectory Errorの測定方法

# [Evaluating Egomotion and Structure-from-Motion Approaches Using the TUM RGB-D Benchmark, IROS'12 Workshop](http://vision.in.tum.de/publications#2012)
A Benchmark for the Evaluation of RGB-D SLAM Systemsとほぼ一緒。

# [Robust Odometry Estimation for RGB-D Cameras, ICRA'13](http://vision.in.tum.de/publications#2013)

Direct MethodによるVisual Odometryの改良版(DVO)で、[コード](https://github.com/tum-vision/dvo_slam)が公開されている。

deskの軌跡の評価(RPE.RMSE[m/s])

| Method|  fr1/desk2 | fr1/desk | fr2/desk | fr2/person|
|:-----------:|:------------:|:------------:|:------------:|:------------:|
|reference |0.3416 |0.5370 |0.0205 |0.0708|
|no weights |0.1003 |0.0551 |0.0231 |0.0567|
|Tukey weights |0.2072 |0.1740 |0.1080 |0.1073|
|t-dist. weights |0.0708 |0.0458 |0.0203 |0.0360|
|t-dist. w.+temporal |0.0687 |0.0491 |0.0188 |0.0345|
|avg. camera velocity |0.413 |0.426 |0.193 |0.121|

カメラの速度が速いとき、5cmくらいの誤差で、遅いときは2cmくらいと思ったらいいだろう。

# [Dense Visual SLAM for RGB-D Cameras, IROS'13](http://vision.in.tum.de/publications#2013)

DVOのSLAM版

RGBのみ、Depthのみ、RGB-D Visual Odometryの評価

|structure |texture |distance |RGB |Depth |RGB+Depth|
|:-----------:|:------------:|:------------:|:------------:|:------------:|:------------:|
|- |x |near |0.0591 |0.2438 |0.0275|
|- |x |far |0.1620 |0.2870 |0.0730|
|x |- |near |0.1962 |0.0481 |0.0207|
|x |- |far |0.1021 |0.0840 |0.0388|
|x |x |near |0.0176 |0.0677 |0.0407|
|x |x |far |0.0170 |0.0855 |0.0390|

fr3のデータセットを用いてRPE(RMSE[m/s])の評価をしている。
建造物とテクスチャのどちらかの場合でRGB-D Odometryがいい結果を出しており、建造物とテクスチャがあるときは、RGBのみがいい結果となった。

RGB-D Odometryのframe-to-frame、frame-to-keyframe、frame-to-keyframeとポーズグラフによる最適化の３つの手法をfr1データセットで評価(RPE.RMSE[m/s])・比較している。

|Dataset |RGB+D |RGB+D+KF |RGB+D+KF+Opt|
|:-----------:|:------------:|:------------:|:------------:|
|fr1/desk |0.036 |0.030 |0.024|
|fr1/desk (v) |0.035 |0.037 |0.035|
|fr1/desk2 |0.049 |0.055 |0.050|
|fr1/desk2 (v) |0.020 |0.020 |0.017|
|fr1/room |0.058 |0.048 |0.043|
|fr1/room (v) |0.076 |0.042 |0.094|
|fr1/360 |0.119 |0.119 |0.092|
|fr1/360 (v) |0.097 |0.125 |0.096|
|fr1/teddy |0.060 |0.067 |0.043|
|fr1/floor |fail |0.090 |0.232|
|fr1/xyz |0.026 |0.024 |0.018|
|fr1/xyz (v) |0.047 |0.051 |0.058|
|fr1/rpy |0.040 |0.043 |0.032|
|fr1/rpy (v) |0.103 |0.082 |0.044|
|fr1/plant |0.036 |0.036 |0.025|
|fr1/plant (v) |0.063 |0.062 |0.191|
|avg. improvement |0% |16% |20%|

提案手法のRGB-D SLAM(frame-to-keyframeとポーズグラフ最適化）はRGB-D Odometryの結果を20パーセントの改善している。

他の最先端手法との比較:ATE(RMSE[m])

|Dataset |# KF |Ours |RGB-D SLAM ICRA'12 |MRSMap |KinFu|
|:-----------:|:------------:|:------------:|:------------:|:-----------:|:-----------:|:-----------:|
|fr1/xyz |68 |0.011 |0.014 |0.013 |0.026|
|fr1/rpy |73 |0.020 |0.026 |0.027 |0.133|
|fr1/desk |67 |0.021 |0.023 |0.043 |0.057|
|fr1/desk2 |93 |0.046 |0.043 |0.049 |0.420|
|fr1/room |186 |0.053 |0.084 |0.069 |0.313|
|fr1/360 |126 |0.083 |0.079 |0.069 |0.913|
|fr1/teddy |181 |0.034 |0.076 |0.039 |0.154|
|fr1/plant |156 |0.028 |0.091 |0.026 |0.598|
|fr2/desk |181 |0.017 |- |0.052 |-|
|fr3/office |168 |0.035 |- |- |0.064|
|average | | 0.034 |0.054 |0.043 |0.297|

平均をみると提案手法が優れている。

# [Dense Tracking and Mapping with a Quadrocopter, UAV-g'13](http://vision.in.tum.de/publications#2013)

UAVに応用

# [CopyMe3D: Scanning and Printing Persons in 3D, GCPR'13](http://vision.in.tum.de/publications#2013)

3Dスキャンして3Dプリント

# [Submap-based Bundle Adjustment for 3D Reconstruction from RGB-D Data, GCPR'14](http://vision.in.tum.de/publications#2014)

マップ最適化にBAを用いる
ATE(RMSE)の評価[m]

|Sequence |No BA |Full 2D |Full 3D |Submap-based 3D BA |RGB-D SLAM ICRA'12|KinFu |Direct RSS'13|
|:-----------:|:------------:|:------------:|:------------:|:-----------:|:-----------:|:-----------:|:-----------:|
|FR1/360 |0.108 |0.099 |0.077 |0.079 |0.079 |0.591 |0.119|
|FR1/desk |0.047 |0.021 |0.022 |0.022 |0.023 |0.068 |0.035|
|FR1/desk2 |0.098 |0.044 |0.030 |0.031 |0.043 |0.635 |0.062|
|FR1/plant |0.048 |0.023 |0.042 |0.043 |0.091 |0.281 |0.043|
|FR1/room |0.275 |0.228 |0.085 |0.086 |0.084 |0.304 |0.078|
|FR1/rpy |0.046 |0.058 |0.027 |0.027 |0.026 |0.081 |0.042|
|FR1/teddy |0.277 |0.060 |0.056 |0.057 |0.076 |0.337 |0.080|
|FR1/xyz |0.015 |0.013 |0.013 |0.013 |0.014 |0.025 |0.023|
|FR2/desk |0.201 |0.080 |0.079 |0.076 |- |- |-|
|FR3/office |0.176 |0.039 |0.036 |0.035 |- |0.061 |0.040|
|average |0.129 |0.066 |0.047 |0.047 |0.054 |0.264 |0.058|

# [Towards Illumination-invariant 3D Reconstruction using ToF RGB-D Cameras](http://vision.in.tum.de/publications#2014)

ToF方式に対応。

# Monocular SLAMの紹介
RGB-D SLAMはKinectやXtionといったRGBとDepthが得られるセンサを用いたSLAMである。ここからは、最近の単眼カメラのみ用いたSLAMを少しだけ紹介する。

# [Semi-Dense Visual Odometry for AR on a Smartphone, ISMAR'14](http://vision.in.tum.de/publications#2014)

full Dense → Semi-Dense VO
チューリッヒ大学の[SVO](https://github.com/uzh-rpg/rpg_svo)のコード有り。

| | | |fr2/xyz ||fr2/desk ||
|:-----------:|:-----------:|:-----------:|:-----------:|:-----------:|:-----------:|:-----------:|
|method |mapping |tracking |(cm/s) |(deg/s) |(cm/s) |(deg/s)|
|PTAM |640×480 |640×480 |8.2 |3.2 |fail |fail|
|ours |640×480 |640×480 |0.50 |0.31 |2.2 |0.96|
|ours |640×480 |320×240 |0.58 |0.32 |3.6 |1.25|
|ours |320×240 |320×240 |0.58 |0.32 |3.3 |0.96|
|ours |320×240 |160×120 |0.62 |0.33 |4.9 |1.38|
|ours |160×120 |160×120 |0.68 |0.37 |fail |fail|
|ours |160×120 |80×60 |1.58 |0.71 |fail |fail|

# [LSD-SLAM: Large-Scale Direct Monocular SLAM, ECCV'14](http://vision.in.tum.de/publications#2014)

SVOのSLAM版、しかも大規模。[コード](https://github.com/tum-vision/lsd_slam)有り。

ATE(RMSE)の評価[cm]

| Error |LSD-SLAM (#KF) |SVO ICRA'14 |PTAM |Dense Visual SLAM IROS'13|RGB-D SLAM ICRA'12|
|:-----------:|:-----------:|:-----------:|:-----------:|:-----------:|:-----------:|
|fr2/desk |4.52 (116) |13.50 |x |1.77 |9.5|
|fr2/xyz |1.47 (38) |3.79 |24.28 |1.18 |2.6|
|sim/desk |0.04 (39) |1.53 |- |0.27 |-|
|sim/slowmo |0.35 (12) |2.21 |- |0.13 |-|

平均は単眼SLAMよりもRGB-D SLAMのほうが良いようだ。誤差は5cm以内。