Visual SLAM研究について、カメラだけでなく、RGB-DやIMUを用いた手法について、個人的に重要と思われる研究に絞って紹介しました。

1. Visual SLAM概観
2019年3月7日
takmin
1

2. 自己紹介
2
株式会社ビジョン＆ITラボ 代表取締役
皆川 卓也（みながわ たくや）
「コンピュータビジョン勉強会＠関東」主催
博士（工学）
略歴：
1999-2003年
日本HP（後にアジレント・テクノロジーへ分社）にて、ITエンジニアとしてシステム構築、プリ
セールス、プロジェクトマネジメント、サポート等の業務に従事
2004-2009年
コンピュータビジョンを用いたシステム/アプリ/サービス開発等に従事
2007-2010年
慶應義塾大学大学院 後期博士課程にて、コンピュータビジョンを専攻
単位取得退学後、博士号取得（2014年）
2009年-現在
フリーランスとして、コンピュータビジョンのコンサル/研究/開発等に従事（2018年法人化）
http://visitlab.jp

3. 本資料について
 本資料は、Visual SLAMという研究分野全体を概観することを
目的に作成しました。
 Visual SLAMの研究は歴史も古く数も膨大のため、以下のよう
な研究に絞って解説を行います。
 筆者が代表的と判断したもの
 ソースコードが公開されている、または誰かの再現実装したコード
が存在しているもの
 今回調査した研究は使用するセンサーによって大きく３つの
分野に分けられます
 カメラを使用したVisual SLAM
 深度センサーを使用したRGB-D SLAM
 カメラおよびIMUを使用したVisual Inertial SLAM
 調査に当たり、九州大学の内山英昭先生にご助言いただきま
した。
3

4. Visual SLAM
4
ロボットの自律移動や、ARにおいてカメラで撮影した画像上に
3Dオブジェクトを重畳表示するためには、撮影対象の三次元形
状とそれに対するカメラの位置と姿勢を正確に求める必要があ
る
Simultaneous Localization And Mapping (SLAM)
Localization
Mapping

5. SLAMの基本原理
1. センサーからランドマークを検出し、Mapへ登録。
Mapに登録されたラン
ドマーク
5

6. SLAMの基本原理
2. センサーを移動させてランドマークを検出する。
今回見つかったランド
マーク
Mapに登録されたラン
ドマーク
6

7. SLAMの基本原理
3. センサーとMap内のランドマークとの対応関係からセン
サーの自己位置を推定。
Mapとの対応が取れた
ランドマーク
今回見つかったランド
マーク
Mapに登録されたラン
ドマーク
7

8. SLAMの基本原理
4. センサーの自己位置を元に、新しく観測されたランド
マークをMapへ登録。２へ戻る。
Mapに登録されたラン
ドマーク
8

9. Visual SLAM
9

10. カメラによるVisual SLAM
カメラによるVisual SLAMは、構築されるMapによってSparse、
Dense、Semi-denseの3パターンに分類できます。
ここではそれぞれのアプローチの中で、代表的かつソース
が存在するものをピックアップして紹介します。
 Sparse
 主に画像から取得したキーポイントを元に疎なMapの構築を
行う手法です。[Klein2007][Mur-Artal2015]
 Dense
 RGBの値を使用して密なMapの構築を行う手法です。
[Newconbe2011][Tateno2017][Zhou2018]
 Semi-Dense
 SparseとDenseの中間にあたる手法です。[Engel2014]
10

11. [基礎知識]Structure from Motion (SfM)
11
 同じ対象を撮影した複数の画像（例えば動画）から、対象
の三次元形状を復元する

12. [基礎知識]Structure from Motion (SfM)
12
 同じ対象を撮影した複数の画像（例えば動画）から、対象
の三次元形状を復元する
バンドル調整を用いて、複数のカメラの相対位置、対象の
三次元点座標を同時に推定する
＜バンドル調整＞
測定結果とパラメータから誤差を計算し、誤差を小さくする方向にパラ
メータを調整していく

13. [基礎知識]バンドル調整
13
1. 三次元点群とカメラパラメータの初期値を設定する
画像から見つけた点の三
次元座標の初期値カメラの位置と焦点距離
の初期値

14. [基礎知識]バンドル調整
14
2. 三次元点群を各カメラの画像平面へ投影

15. [基礎知識]バンドル調整
15
3. 投影された点と観測した点との誤差の総和を算出
投影された点
観測点
誤差

16. [基礎知識]バンドル調整
16
4. 誤差が小さくなる方向へ特徴点の三次元座標とカメラ
パラメータを調整 （収束するまで２から繰り返す）

17. [基礎知識]Multi-View Stereo (1/3)
17
SfMで求めた形状は、特徴点の
三次元位置のみ
SfMで求めたカメラパラメータを元に各カメラから
物体表面までの距離を画素ごとに算出（デプス
マップ）
全てのカメラのデプスマップを統合して
物体の形状を算出

18. [基礎知識]Multi-View Stereo (2/3)
18
 カメラ１の画素Aのデプスをカメラ２から算出する例
 カメラ１の焦点から画素Aへの視線（エピポーラ線）上をカメラ
２の画像から探索し、最も類似したテクスチャを求める。
(Photo Consistency)
カメラ１ カメラ2
Aのエピポーラ線
A
d
d 距離
類似度

19. [基礎知識]Multi-View Stereo (3/3)
19
 各カメラから求めたデプスマップを統合して三次元モデ
ルを生成

20. [Klein2007]PTAM (1/3)
 マーカーレスで単眼カメラのPose TrackingをCPU上でリアルタイムで
行うSparseVisual SLAM
 TrackingとMappingを別スレッドで実行
 全フレームではなく、KeyFrameを元にMapping
 http://www.robots.ox.ac.uk/~gk/PTAM/
KeyFrame
KeyFrameから
構築されたMap
20

21. [Klein2007]PTAM (2/3)
 Mapping
 KeyFrameをTrackingスレッドから受け取ったら処理開始
 MapとKeyFrameのキーポイントとの対応を算出し、新しいMap
Pointsを追加
 バンドル調整で再投影誤差を最小化することでKeyFrameの
PoseとMap Pointsの3D座標算出
 Tracking
 画像ピラミッドを作成し、Coarse-to-Fineに処理
 FASTでキーポイントを検出し、周辺画像パッチを取得
 Map PointsとKeyPointsを対応付けることでPose推定
21

22. [Klein2007]PTAM (3/3)
 合成動画上でのEKF-SLAMとの性能比較
PTAMで生成したMap EKF-SLAMで生成したMap 軌跡のGroundTruthとの比較
 実験環境
 デスクトップPC＋カメラ
 Intel Core 2 Duo 2.66 GHz
Map Points = 4000の時のトラッキング速度
22

23. [Newcombe2011]DTAM (1/3)
 密な三次元（深度）の復元とカメラのトラッキングを同時に行う、
Dense MonocularVisual Slamの代表的手法
 以下の筆者以外の再現実装が公開されている
 https://github.com/anuranbaka/OpenDTAM
 https://github.com/TeddybearCrisis/OpenDTAM-3.1
 KeyFrameの各画素と同じ色の画素を周辺フレームから（エピポーラ
線上を）探索することで深度を推定 (Photo Consistency)
23

24. [Newcombe2011]DTAM (2/3)
 Dense Mapping
 以下のエネルギーを最小化する深度𝝃を推定
画像全体で積分
データ項：
KeyFrameの画素と周辺フレーム
の画素との一致度（前頁参照）
平滑化項：
深度がスムーズになるよ
うに（ただしエッジは保存）
 Dense Tracking
 2段階のテンプレートマッチングによりPose推定
1. 前フレームと現フレームとの画像同士のマッチングにより、回転を
算出
2. Dense Mapを元に、現フレームの画素を前フレームへ投影し、マッチ
ングをとることで回転を詳細化しつつ並進を算出（6DoF）
24

25. 25
[Newcombe2011]DTAM (3/3)
 Intel Core i7 quad-core CPU
 NVIDIA GTX 480 GPU
 入力画像：640 x 480, 24bit RGB, 30Hz
カップ付近で高速に前後動かした画像に対し、PTAMと比較

26. 26
[Engel2014]LSD-SLAM (1/3)
 特徴点ではなく（勾配の大きい）画素の値を直接使って
Semi-DenseなMap構築およびPose推定
 Map Optimizationにより大規模なマップを構築可能
 https://vision.in.tum.de/research/vslam/lsdslam
KeyFrameとの画素の差が
最小となるようPose推定
KeyFrameを生成し、その
深度を推定
Loop ClosingによりMap全
体を最適化

27. 27
[Engel2014]LSD-SLAM (2/3)
 Tracking
 濃度勾配の高い画素のみPose推定に使用（Semi-Dense）
 深度を使ってKeyFrameの画素を現フレームに投影し、差分を最小
化するようPose推定 (Direct法)
 Depth Map Estimation
 Poseの変化が閾値を超えたらKeyFrame生成
 KeyFrameの深度初期値を前KeyFrameの深度を投影して生成
 追跡フレームとKeyFrameとのベースラインステレオで深度を補正*
 Map Optimization
 KeyFrame生成時近傍のKeyFrameおよび類似KeyFrameを取得し、そ
れぞれLoopかを判別
 Loopが存在する場合、２つのKeyFrameの画素と深度から相対Pose
を求め、それをLoop上を伝播させて最適化(Graph Optimization)
*J. Engel, J. Sturm, and D. Cremers. Semi-dense visual odometry for a monocular camera. In IEEE International Conference on
ComputerVision (ICCV), December 2013

28. [Engel2014]LSD-SLAM (3/3)
[9]Engel, J., Sturm, J., Cremers, D.: Semi-dense visual odometry for a monocular
camera. In: Intl. Conf. on ComputerVision (ICCV) (2013)
[15]Klein, G., Murray, D.: Parallel tracking and mapping for small AR workspaces. In: Intl.
Symp. on Mixed and Augmented Reality (ISMAR) (2007)
[14]Kerl, C., Sturm, J., Cremers, D.: Dense visual SLAM for RGB-D cameras. In: Intl.
Conf. on Intelligent Robot Systems (IROS) (2013)
[7]Endres, F., Hess, J., Engelhard, N., Sturm, J., Cremers, D., Burgard,W.:An evaluation of
the RGB-D slam system. In: Intl. Conf. on Robotics and Automation (ICRA) (2012)
TUM-RGBDベンチマーク（軌跡の二乗誤差（cm））
28

29. 29
[Mur-Artal2015]ORB-SLAM (1/4)
 単眼V-SLAMをTracking、 Local Mapping、Loop Closingの３つのス
レッドを並列に動作させることで、リアルタイムに実現
 全スレッドでORB特徴を利用
 https://github.com/raulmur/ORB_SLAM2
入力フレームの
カメラPoseを推定
Sparseな3D
Map(点群)を作成
ループを検出しカ
メラPoseの誤差
の蓄積を除去
選定された
KeyFrame
点群＋Poseグラフ
ORBとBoVWで
KeyFrame検索する
ためのDB
共有メモリ

30. [Mur-Artal2015]ORB-SLAM (2/4)
 MAP
 点(XYZ)とそれに紐づいた代表ORB特徴
 各キーフレームの全ORB特徴を保持
 Place Recognition:
 ORBによるBags-of-Wordsでクエリー画像に近いキーフレームを検
索するデータベース
 追跡失敗時やMapping時に対応点が見つからない時、Loopを探す
時などに利用
 Tracking
 入力フレームのORBとMapとの対応関係でPose推定
 Local Mapping
 KeyFrameから、Place Recognition DB、グラフの更新と3次元点群の
生成/更新（バンドル調整）
 Loop Closing
 Loop候補を算出し、KeyFrame間の相似変換を伝播
30

31. [Mur-Artal2015]ORB-SLAM (3/4)
 KeyFrameをNode、フレーム間で共通して見えるORB特徴が
閾値以上のものをエッジとしてグラフ化
SpanningTree
に強いエッジを
追加
15個以上の共
有点を持つ場
合エッジを生
成
Covisibility
Graphから作成
した全域木
(SpanningTree)
31

32. [Mur-Artal2015]ORB-SLAM (4/4)
 評価実験
 Intel Core i7-4700MQ(4 cores
@2.40GHz) + 8Gb RAM
TUM-RGBDベンチマークの結果
処理時間
32

33. [Mur-Artal2016]ORB-SLAM2 (1/2)
ORB-SLAMの入力をステレオカメラおよびRGBDセンサーへ拡張
 https://github.com/raulmur/ORB_SLAM2
 左右のカメラのKeyPointを（RGBDの場合は疑似的に）取得し、
深度に合わせてcloseとfarに分類
 KeyFrameはfarに対してcloseなKeyPointが十分な時に生成
 TrackingやLocal Mappingを増え
たKeyPointに対して行う
 Stereo/RGBDではScale Driftが
避けられるため、Loop Closing
時に相似変換ではなく回転/平
行移動を用いる
 Dense Mapの構築は単純に
KeyFrameに紐づいた点群を投
影するだけ（Not Real-time） Stetreo/RGBDカメラからのキーポイント取得
33

34. [Mur-Artal2016]ORB-SLAM2 (2/2)
 評価実験
 Intel Core i7-4790 + 16Gb RAMにおいて、常にセンサーのフ
レームレート以下で動作
EuRoCデータセットで
のRMSE（m）
TUM-RGBDデータセット
でのRMSE(m)
34

35. [Tateno2017]CNN-SLAM (1/3)
 LSD-SLAMの深度推定をDeep Neural Networkによる深度推
に置き換えることでDenseなMapを構築。
 Visual SLAMの弱点である、テクスチャがないケース、スケール不定、
回転に弱いなどの問題を補間
 Semantic Labelも付与
 SLAMにCNNを導入した最初期の論文
 IITによる再実装コード
 http://iitmcvg.github.io/projects/CNN_SLAM
CNNによる深度と
Semantic Labelの推定
LSD-SLAM
35

36. [Tateno2017]CNN-SLAM (2/3)
 Camera Pose Estimation
 現フレームの画素を前キーフレーム上へ投影した時の差が最
小となるPoseを推定(Direct法)
 LSD-SLAM同様、輝度勾配の高い領域
 投影時にCNNで推定した深度情報を使用
 LSD-SLAMではKey-Frame間のステレオで深度推定
 CNN Depth Prediction & Semantic Segmentation
 Laina, I., Rupprecht, C., Belagiannis,V.,Tombari, F., & Navab, N.
(2016). Deeper Depth Prediction with Fully Convolutional
Residual Networks. IEEE International Conference on 3DVision.
 各KeyFrameに対し深度推定
 LSD-SLAMと同様にbaseline stereoを用いて深度を補正
36

37. [Tateno2017]CNN-SLAM (3/3)
 ICL-NUIM datasetとTUM datasetによる軌跡と深度の精度評価
以下の環境でリアルタイム
• Intel Xeon CPU at 2.4GHz with 16GB of RAM
• Nvidia Quadro K5200 GPU with 8GB of VRAM
37

38. 38
[Zhou2018]DeepTAM (1/3)
 TrackingとMappingの推定両方を学習ベースで行うDense
Monocular SLAM
 https://github.com/lmb-freiburg/deeptam
 Mapping
 KeyFrameとTrackingフレームとのPhoto Consistencyを入力とし、深
度を出力するネットワーク
 KeyFrame各画素の深度をサンプリングし、深度とPoseを元にTrackingフレー
ムの画像パッチをKeyFrameへ投影することでPhoto Consistency算出
 Coarse-to-Fineに推定
深度のサンプ
リング間隔
深度毎のPhoto
Consistency 深度
繰り返し処理で徐々
にサンプリング深度
を絞り込む

39. [Zhou2018]DeepTAM (2/3)
 Tracking
 KeyFrameと深度から、Pose 𝑇 𝑉から見たKeyFrame画像と深度をレンダリング
 レンダリングしたKeyFrameと深度、および現フレームを入力とし、Pose変化を予測
するネットワーク
 学習時はOptical Flowも同時に学習
 ネットワークを多段にして、画像ピラミッドを入力とすることでCoarse-to-FineにPose
推定
39

40. [Zhou2018]DeepTAM (3/3)
 処理速度についての記載なし
TUM RGB-D Datasetで評価
• Translational RMSE (m/s)
• TrackingはFrame-to-
KeyFrameでのエラー
10フレーム使用した時の推定
深度の定性評価
40

41. 参考文献 (カメラによるVisual SLAM)
 [Klein2007]Klein, G., & Murray, D. (2007). ParallelTracking and Mapping for Small
AR Workspaces. In IEEE and ACM International Symposium on Mixed and Augmented
Reality, ISMAR.
 [Newcombe2011]Newcombe, R.A., Lovegrove, S. J., & Davison,A. J. (2011). DTAM:
DenseTracking and Mapping in Real-Time. In International Conference on Computer
Vision.
 [Engel2014]Engel, J., Schops,T., & Cremers, D. (2014). LSD-SLAM: Large-Scale Direct
monocular SLAM. In European Conference on ComputerVision
 [Mur-Artal2015]Mur-Artal, R., Montiel, J. M. M., & Tardos, J. D. (2015). ORB-SLAM:A
Versatile and Accurate Monocular SLAM System. IEEETransactions on Robotics, 31(5),
1147–1163.
 [Mur-Artal2016]Mur-Artal, R., & Tardos, J. D. (2016). ORB-SLAM2: an Open-Source
SLAM System for Monocular, Stereo and RGB-D Cameras. ArXiv, (October).
Retrieved from
 [Tateno2017]Tateno, K.,Tombari, F., Laina, I., & Navab, N. (2017). CNN-SLAM : Real-
time dense monocular SLAM with learned depth prediction. In IEEE Conference on
ComputerVision and Pattern Recognition.
 [Zhou2018]Zhou, H., & Ummenhofer, B. (2018). DeepTAM : DeepTracking and
Mapping. In European Conference on ComputerVision.
41

42. RGB-D SLAM
42

43. RGB-D SLAM
RGB-D SLAMはKinectのようなRGB-Dセンサーを用い、
DenseなMapを構築する手法です。Pose推定をICPやNDT
などの点群/深度同士のマッチング（Scan Matching）と、画
像のキーポイントや画素間のマッチングの両方を使用して
行います。
ここでは、初期に提案された深度のみを用いる手法
[Newcombe2011]と、画像の情報も併用する手法
[Kerl2013][Whelan2016][Dai2017]について紹介します。
43

44. [基礎知識]Iterated Closest Point (ICP)
 点とモデル同士をマッチングさせるためのアルゴリズム
 近傍点への対応付けと移動を交互繰り返す
 点とモデルをマッチングさせるためのに回転/並進運動が得ら
れる
モデル
入力点群
44

45. [基礎知識]Iterated Closest Point (ICP)
1. 入力の各点と最も近いモデル上の点同士を紐づける
45

46. [基礎知識]Iterated Closest Point (ICP)
2. 対応付けた点同士の距離の2乗和が最小となるように
入力点群を移動させる
46

47. [基礎知識]Iterated Closest Point (ICP)
3. 再び点とモデルと最も近い点同士を対応付け、対応点
同士の距離が最小となるように入力点群を移動させる
47

48. [基礎知識]Iterated Closest Point (ICP)
4. 収束するまで繰り返す
48

49. [Newcombe2011]KinectFusion (1/3)
 距離センサーにより取得した深度を合成して、リアルタイムに
3Dシーンの再構成とセンサーのPoseを推定（RGBは不使用）
 OpenCV4.0以降のRGBDモジュール（contrib）に含まれている
 https://github.com/opencv/opencv_contrib
レンダリングした
Surfaceと点群との
ICPでPose推定
現在のセンサーの
視野からSurfaceをレ
ンダリング
深度データから点群
と法線を計算
マッチングした点群
を元にSurface情報
を更新
49

50. [Newcombe2011]KinectFusion (2/3)
 Surfaceの情報をTruncated Signed Distance Function
(TSDF)形式で持たせる
 Surfaceからの距離に応じてVoxelに値を割り振り、手前を正、
奥側を負にしたVoxel空間
 ICPでマッチングしたSurfaceを元に、TSDFの値を更新していく
50

51. 51
[Newcombe2011]KinectFusion (3/3)
 リアルタイムには要GPU
 Voxel解像度と処理時間の関係

52. [Kerl2013]DVO (1/3)
 RGB-Dカメラを用い、深度と画素両方の投影誤差が最小とな
るように最適化するVisual SLAMの提案
 画像𝛪1に対する画像𝛪2のPoseが𝑔で、三次元点pの画像𝛪1上の座標
をx、深度を𝛧1とした時、画像𝛪2上の座標x′
と深度𝛧2は一意に求まる
 現フレームのRGB-D画像をKeyFrameに投影した時の画素の差、お
よび深度の差が最小となるPose 𝑔∗
を求めることでTracking (Frame-
to-Frame Tracking)
 https://github.com/tum-vision/dvo
52

53. [Kerl2013]DVO (2/3)
 投影誤差の分散から求め
たエントロピー（の比）を用
いて、KeyFrameを選択
 KeyFrameをノードとした
Pose Graphを作成
 距離に基づいてLoop候補
を検出し、エントロピーに基
づき候補を決定し、Loop全
体の誤差が最小となるよう
に各Poseを補正 (Loop
Closing)
53

54. [Kerl2013]DVO (3/3)
 Trackingは1フレームあたり約32ms
 Intel Core i7-2600 3.4GHz + 16GB RAM
 Trackingは独立したスレッド
 TUM RGB-D DatasetでLocalization評価(m)
54

55. [Whelan2016]ElasticFusion (1/4)
 Real-time DenseVisual SLAM (要GPU)
 https://github.com/mp3guy/ElasticFusion
 Pose GraphよりもシンプルなDeformation Graphを用いて高頻
度にLoop ClosingすることでMapを正確に保つ
 光源推定で照明の影響を除去
(i)初期は全てのデータがActive; (ii)古いデータのうち長く観測されていない領域はInavtive(灰色); (iii)カメラが
Inactive領域をとらえたら、Local Loop Closingを行いSurfaceを統合して再びActiveに; (iv)更にinactive領域で
Loop Closing;(v)新しい領域をスキャン;(vi)inactive領域でLoop Closingが行えないほどずれている; (vii)赤矢印
がinactiveとactiveでのずれ; (viii)Global Loop Closingでずれを修正; (ix)更にLocal Loop Closingでinactive領域
をactiveに; (x)最終マップ
55

56. [Whelan2016]ElasticFusion (2/4)
 Mapping
 シーン(モデル)を円盤状のSurfel(座標,法線,半径,色,重み)の集合ℳで表
現
 重みの初期値は光学中心に近いほど高く設定
 マッチングしたSurfel同士の各パラメータを加重平均することでアップデー
ト (Registration)
 Tracking
 Model ℳを時間閾値𝛿𝑡より前をActive、後をInactiveに分割し、Activeな
SurfelをTrackingに利用
 入力フレーム（RGBD）とActive Modelからレンダリングで生成した前フ
レームのRGBD画像との距離(Point-to-Plane ICP)および色のマッチング
でPose算出
 Deformation Graph
 ノードとしてSurfelの位置と取得時刻、リンクは隣接Surfel
 ただしグラフのノード数 ℊ ≪ ℳ
 Loop Closingに利用
56

57. 57
[Whelan2016]ElasticFusion (3/4)
 処理の流れ
(i)取得したデプス𝐷𝑡
𝑙
とカラー画像𝐶𝑡
𝑙
を前フレームのActive Modelから生成した(෡𝐷𝑡−1
𝑎
, መ𝐶𝑡−1
𝑎
)へFittingさせることで
Pose 𝐏𝑡 を算出し、新しいActiveフレーム (𝐷𝑡
𝑎
, 𝐶𝑡
𝑎
)を生成。生成したフレームをFern DBから検索; (ii)DBからフ
レームℇ𝑖𝑑が取得できた場合、それを現Activeフレームへ登録を試み、うまくいったらDeformationによりモデル
෡ℳとPose ෡𝐏𝑡を更新(Global Loop Closing); (iii)DBからの取得や更新に失敗した場合、Pose 𝐏𝑡からInactiveフレー
ム(𝐷𝑡
𝑖
, 𝐶𝑡
𝑖
)を生成し、現Active Modelへ登録を試みる。成功の場合、モデルをDeformation (Local Loop Closing);
(iv)現カメラデータを最新の更新モデル ෡ℳへ統合し、最新のActiveフレーム(෡𝐷𝑡
𝑎
, መ𝐶𝑡
𝑎
)を生成。次フレームの
Trackingに利用しつつ、Fern DB ෡ℇへ登録を試みる

58. [Whelan2016]ElasticFusion (4/4)
TUM RGB-D DatasetでLocalization評価
ICL-NUIM DatasetでMapping評価
Surfelと処理速度の関係
 Intel Core i7-4930K CPU at 3.4GHz,
32GB of RAM
 nVidia GeForce GTX 780 Ti GPU
with 3GB mem
58

59. [Dai2017]BundleFusion (1/4)
 全入力フレームを使用してリアルタイムにDenseな3Dモ
デルを構築/更新（要GPU）
 Sparse-to-Denseにフレーム間のマッチング
 Local-to-GlobalにフレームのPose推定
 全フレームを最適化するためLoop Closing不要
 http://graphics.stanford.edu/projects/bundlefusion/
キーポイント抽
出とマッチング
連続フレームをま
とめてchunkを構
成し、全フレーム
のPose推定
chunkからキーフ
レームを抽出し、
Pose推定
各フレームのPoseを
元にDense Map構築
59

60. [Dai2017]BundleFusion (2/4)
 Correspondence Search
 SIFTで各フレームからキーポイント抽出
 キーポイント周辺のColor、Depth及びNormalを取得し、幾何
情報とともに対応点の検証に利用
 Local Pose Optimization
 入力を複数の連続フレームに分割(1 chunkあたり11枚、オー
バーラップ1枚）
 chunk内の全フレーム間の対応点を算出
 chunk内の全フレームの相対Poseを推定
 Global Pose Optimization
 各chunkの最初のフレームをKeyFrameとし、chunk内の特徴を
統合
 全KeyFrameを同様の方法でPose推定
60

61. [Dai2017]BundleFusion (3/4)
 Optimization
 Local/Global Optimizationで共通の処理
 Keypoints Matchingを元にPose推定後、色情報とデプス情報の
項を加えてより詳細なPose推定（Sparse-to-Dense）
 Integration/Deintegration
 TSDFとVoxel Hashingを用いて、各フレームのPoseとRGBD
データから3D Mapを構築
 Local Pose Optimizationの結果を元にリアルタイムでMapを更
新し、Global Pose Optimizationの結果を、Pose変化の大きい
個所から優先的に反映
61

62. [Dai2017]BundleFusion (4/4)
Structure Sensorで取得した屋内データでの比較 ICL-NUIM DatasetでMapping評価
TUM RGB-D DatasetでLocalization評価 パフォーマンス評価
 Core i7 3.4GHz CPU (32GB RAM)
 NVIDIA GeForce GTXTitan X (for reconstruction)
 NVIDIA GTXTitan Black (for search / global pose optimization)
62

63. 参考文献 (RGB-D SLAM)
 [Newcombe2011]Newcombe, R. a., Davison,A. J., Izadi, S., Kohli,
P., Hilliges, O., Shotton, J., … Fitzgibbon,A. (2011). KinectFusion:
Real-time dense surface mapping and tracking. IEEE International
Symposium on Mixed and Augmented Reality.
 [Kerl2013]Kerl, C., Strum, J., & Cremers, D. (2013). DenseVisual
SLAM for RGB-D Cameras. In IEEE/RSJ International Conference
on Intelligent Robots and Systems (IROS).
 [Whelan2016]Whelan,T., Salas-Moreno, R. F., Glocker, B.,
Davidson,A. J., & Leutenegger, S. (2016). ElasticFuion: Real-Time
Dense SLAM and Light Source Estimation. The International
Journal of Robotics Research.
 [Dai2017]Dai,A., Niessner, M., Zollhofer, M., Izadi, S., & Theobalt,
C. (2017). BundleFusion: Real-time Globally Consistent 3D
Reconstruction using On-the-fly Surface Re-integration. ACM
Transactions on Graphics (TOG).
63

64. Visual Inertial SLAM
64

65. Visual Inertial SLAM
VI-SLAMはIMUを用いて、Visual SLAMの精度を高める手法
です。大きくフィルタベースの手法と最適化ベースの手法に
分けられます。
ここではそれぞれのアプローチの中で、代表的かつソース
が存在するものをピックアップして紹介します。
 フィルタベース
 Kalman FilterやParticle Filterなどを使用して、IMU/カメラPose
の確率遷移を計算[Bloesch2017] [Schnider2017]
 最適化ベース
 バンドル調整などを使用して、誤差の最小化を行うことで、カメ
ラ/IMUのPoseを推定[Leutenegeer2015] [Qin2018]
65

66. [基礎知識]Inertial Measurement Unit (IMU)
 ジャイロスコープ（角速度）
 ෝ𝜔 𝑡 = 𝜔 𝑡 + 𝑏 𝜔 𝑡
+ 𝑛 𝜔
 加速度計（加速度＋重力）
 ො𝑎 𝑡 = 𝑎 𝑡 + 𝑏 𝑎 𝑡
+ 𝐑 𝑤
𝑡
𝑔 𝑤
+ 𝑛 𝑎
 磁場（緯度経度）
 屋内の金属や電気製品などに影響を受ける
 調査した範囲ではVI-SLAMではほとんど使われていない
 角速度/加速度のノイズは平均0のガウス分布、バイアス
の微分は平均0のガウス分布（ランダムウォーク）
測定結果 真値 バイアス ノイズ
測定結果 真値 バイアス ノイズ重力向き
[https://jp.mathworks.com/help/fusion/gs/mod
el-imu-gps-and-insgps.html]より転載
66

67. [Leutenegeer2015]OKVIS (1/3)
 単眼/ステレオ画像から取得したキーポイントとIMUから
取得した情報を密に統合(tightly-coupled)し、非線形最適
化
 従来はEKFなどのフィルタベース
 最適化は複数フレームをバッチ(SlidingWindow)として行う
 https://github.com/ethz-asl/okvis
カメラのみ カメラ＋IMU
67

68. [Leutenegeer2015]OKVIS (2/3)
 カメラとIMU由来のコスト関数を最小化
i: カメラID, k: フレームID, j:キーポイント
𝒆 𝑟
𝑖,𝑗,𝑘
:再投影誤差 𝒆 𝑠
𝑘:位置/姿勢予測結果との誤差
 位置/姿勢の予測
 カメラとIMUの計測は非同期
 時刻kで算出した位置/姿勢から、IMUで計測した速度等を用いて
k+1での位置/姿勢を予測
68

69. [Leutenegeer2015]OKVIS (3/3)
 ETH Building Datasetでの評価結果
69

70. [Qin2018]VINS-Mono (1/4)
 ロバストで汎用的なVI-SLAMを様々な工夫で実現
 IMU Pre-integration, Estimator Initialization, Tightly-coupled
VIO, Efficient Global Optimization
 最適化ベースの手法
 https://github.com/HKUST-Aerial-Robotics/VINS-Mono
70

71. [Qin2018]VINS-Mono (2/4)
 Measurement Preprocessing
 Good Feature toTrackとKLTで画像からキーポイントを取得/追
跡
 ジャイロを利用し、視差が十分かつ回転の少ない画像をキーフレー
ムとして取得
 OKVIS同様Poseの予測値と測定値の差を元に最適化
 フレーム間で取得したIMUの加速度及び角速度に関わる積分値を事
前計算しておくことで繰り返し計算を高速化
 Initialization
 画像を元に5点アルゴリズムなどでPoseの初期値を算出し、
IMUの情報でスケール、重力、速度等の補正を行う
71

72. [Qin2018]VINS-Mono (3/4)
 LocalVisual-inertial Odometry with Relocalization
 キーポイントの再投影誤差（画像）と角速度/加速度を用いた
Poseの予測値と実測値との誤差（IMU）を元に非線形最適化
 キーフレームからBRIEF特徴を抽出しDBへ格納し、Loop
DetectionやRelocalizationに使用
 Global Pose Graph Optimization
 キーフレームごとにPoseを算出し、グラフ化
 IMUによりrollとpitchは計測されるため、4自由度のみ用いて
グラフ全体を最適化（Poseを補正）＝Loop Closing
72

73. [Qin2018]VINS-Mono (4/4)
 Intel i7-4790 CPU (3.60GHz)でリアルタイム
 EuRoC MAVVisual Inertial Datasetで評価
73

74. [Bloesch2017]ROVIO (1/4)
 カメラとIMUの情報を密に統合して自己位置推定を行う
フィルタベースの手法
 Iterated Extended Kalman Filter (IEKF)で推定
𝒙 𝑘 = 𝑓 𝒙 𝑘−1, 𝒘 𝑘−1
𝒚 𝑘 = ℎ 𝒙 𝑘, 𝒏 𝑘
時刻kの内部状態
ガウスノイズ
観測結果
状態遷移
観測
𝒙 𝑘 : センサー位置、速度、回転、加速度バイアス、角速度バイアス、
追跡ランドマーク、etc
𝒚 𝑘: 画像上のランドマーク測定誤差
• IMUの情報を用いて𝑓により状態遷移(𝒙 𝑘−1
+
→ 𝒙 𝑘
−
)
• 画像の情報を用いて誤差𝒚 𝑘が最小になるよう内部状態
更新(𝒙 𝑘
−
→ 𝒙 𝑘
+
)74

75. [Bloesch2017]ROVIO (2/4)
 状態遷移𝑓
 IMUの角速度や加速度などの情報
を用いて姿勢等のパラメータのガウ
ス分布パラメータを更新
 観測ℎ
 ランドマークの予測位置における画
像パッチ（マルチスケール）同士の差
分を元に誤差𝒚 𝑘を算出
 Kanade-Lucas-Tomasi (KLT)で誤差を
最小にするようランドマークの移動
量を修正
 IEKF内に組み込むことで、パラメータの
確率分布を更新
 コード
 https://github.com/ethz-asl/rovio
75

76. [Bloesch2017]ROVIO (3/4)
繰り返し処理による画
像パッチのマッチング
パラメータ更新
更新量算出
パッチのWarp
パッチのマッチング誤差
QR分解による軽量化
パッチ抽出とWarp
状態遷移
スコア算出とラ
ンドマーク更新
76
共分散行列
Warp Matrix
角速度,加速度

77. [Bloesch2017]ROVIO (4/4)
移動距離とエラー平均/分散
(位置)
EuRoC DatasetV1_03 1.4km long circular dataset 1.4km long circular dataset
移動距離とエラー平均/分散
(位置)
移動距離とエラー平均/分散
(Yaw角)
 EuRoC及びCircular Datasetによる比較評価
 Intel Core i7 (2.4MHz)
77

78. [Schnider2017]maplab (1/3)
 VI-SLAM研究のためのオープンソースフレームワーク
 https://github.com/ethz-asl/maplab
 ROVIOを拡張したROVIOLIというVI-SLAMを搭載
 様々なMapを用いたアルゴリズムをプラグイン可能
 Mapを用いた位置推定、 Loop Closing、可視化、マージ、スパース
化、MultiView Stereoや、アルゴリズム評価などの機能を提供
a. ROVIORIによる自己位置推定とMap生成
b. コンソールからMapを読み込み、プラグインしたアルゴリズムを適用
c. ROVIORIによるMap上の自己位置推定
78

79. [Schnider2017]maplab (2/3)
 ROVIOLI
 ROVIOにMapping機能を追加
 BRISKまたはFREAKでキーポイントの検出と追跡（Feature Tracking）
 3D Mapに対する2D画像のグローバル位置推定(Frame Localization)
 グローバルな位置とROVIOによる自己位置推定を統合
 各モジュールの出力をMap Builder上で統合し、Visual-inertial Mapを生成
79

80. 80
[Schnider2017]maplab (3/3)
 EuRoC machine hall datasetでの評価
VIWLS
ROVIOLIの結果にLoop Closingを加えたもの
 処理時間
 Intel Xeon E3-1505M
@2.8Ghz (8 cores)

81. 参考文献 (Visual Inertial SLAM)
 [Leutenegeer2015]Leutenegeer, S., Furgale, P., Rabaud,V., Chli,
M., Konolige, K., & Siegwart, R. (2015). Keyframe-BasedVisual-
Inertial SLAM Using Nonlinear Optimization. The International
Journal of Robotics Research, (september).
 [Qin2018]Qin,T., Li, P., & Shen, S. (2018).VINS-Mono:A Robust
andVersatile MonocularVisual-Inertial State Estimator. IEEE
Transactions on Robotics, 34(4), 1004–1020.
 [Bloesch2017]Bloesch, M., Burri, M., Omari, S., Hutter, M., &
Siegwart, R. (2017). IEKF-basedVisual-Inertial Odometry using
Direct Photometric Feedback. The International Journal of
Robotics Research, 36(1053–1072).
 [Schnider2017]Schnider,T., Dymczyk, M., Fehr, M., Egger, K.,
Lynen, S., Gilitschenski, I., & Siegwart, R. (2017). maplab:An
Open Framework for Research inVisual-inertial Mapping and
Localization. IEEE Robot, 3, 1418–1425.
81

82. まとめ
 本調査では、Visual SLAMという技術分野について調査
いたしました。
 調査は、カメラによるVisual SLAM、RGB-D SLAM、Visual
Inertial SLAMの三分野について、まずその基礎知識につ
いて記載した後、ソースコードが存在し、かつ特に代表的
と筆者が独断で判断した研究について、その概要を記載
いたしました。
 近年はDeep Learningを用いたSLAMが特に活発に研究
されています。
82

83. まとめ
83
手法 センサー GPU Tracking Mapping Loop Closing
PTAM Camera 不要 Frame-to-Model Sparse 無し
DTAM Camera 要 Frame-to-Model Dense 無し
LSD-SLAM Camera 不要 Frame-to-Frame Semi-Dense 有り
ORB-SLAM Camera 不要 Frame-to-Model Sparse 有り
ORB-SLAM2 (Stereo) Stereo Camera 不要 Frame-to-Model Sparse 有り
ORB-SLAM2 (RGB-D) RGB-D 不要 Frame-to-Model Dense 有り
CNN-SLAM Camera 要 Frame-to-Frame Dense 有り
DeepTAM Camera 要 Frame-to-Frame Dense 無し
Kinect Fusion RGB-D 要 Frame-to-Model Dense 無し
DVO RGB-D 不要 Frame-to-Frame Dense 有り
ElasticFusion RGB-D 要 Frame-to-Model Dense 有り
BundleFusion RGB-D 要 Frame-to-Model Dense 有り
OKVIS Camera+IMU 不要 Frame-to-Model Sparse 有り
Vins-Mono Camera+IMU 不要 Frame-to-Model Sparse 有り
ROVIO Camera+IMU 不要 Frame-to-Model Sparse 無し
ROVIOLI (maplab) Camera+IMU 不要 Frame-to-Model Sparse 有り