第51回コンピュータビジョン勉強会＠関東「コンピュータビジョン - 広がる要素技術と応用 - 」読み会　"4.4 未知な環境下におけるカメラの位置姿勢推定"発表資料

1. 第51回CV勉強会「第４章 拡張現実感のための
コンピュータビジョン技術」
4.4 未知な環境下におけるカメラの位置姿勢推定
2019/02/27 takmin

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

3. 昔作ったもの
3
 ARウェルカムボード （2010年）
 https://www.youtube.com/watch?v=KgQguj78qMA
 ソース： https://github.com/takmin/OpenCV-Marker-less-AR
今日はこの話はしません

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. カメラ位置の推定
9
 点の三次元座標と、それを投影した画像上の座標との
対応関係が６点以上明らかなとき、カメラ座標の世界座
標に対する回転𝑹 と並進𝑻を算出できる
 回転：３自由度＋並進３自由度＝６自由度
𝑷 = 𝑲 𝑹 𝑻
𝑠𝒖 = 𝑷𝑿 𝒘
(4.9)
回転 並進
𝒖
𝑿 𝒘
三次元
座標
画像上
の座標
𝑷

10. 三次元点群の推定
10
 Pose（回転𝑹 と並進𝑻 ）が既知の２台以上のカメラについ
て、画像上の点の対応関係が分かっているとき、その点
の三次元座標が求まる
𝑠𝒖 = 𝑷𝑿 𝒘
𝑠′𝒖′ = 𝑷′𝑿 𝒘
(4.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. Visual SLAMの基本原理
1. カメラからキーポイントを検出。三次元座標が既知の
点を用いて𝑹0, 𝑻0を算出。
三次元座標が既知
17
新たに検出されたキー
ポイント

18. Visual SLAMの基本原理
2. カメラを移動させてキーポイントを検出。三次元座標が
既知の点を用いて𝑹1, 𝑻1を算出。
今回見つかったランド
マーク
18
三次元座標が既知
前フレームで検出され
たキーポイント

19. Visual SLAMの基本原理
3. 前フレームと現フレームの両方で観測されたキーポイ
ントの三次元座標を算出。
今回見つかったランド
マーク
19
三次元座標が既知
前フレームで検出され
たキーポイント

20. Visual SLAMの基本原理
4. カメラを移動させてキーポイントを検出。三次元座標が
既知の点を用いて𝑹2, 𝑻2を算出。以下、2と3を繰り返
す
今回見つかったランド
マーク
20
三次元座標が既知
前フレームで検出され
たキーポイント

21. Direct法を用いたSLAM
21
SfMで求めた形状は、特徴点の
三次元位置のみ
画素の値を直接使って画像同士をマッチングす
ることで、密な3D Mapを求める
min
𝑴
𝑰 − 𝑰 𝑟(𝑴)
外部パラメータ

22. Direct法: Photo Consistency
22
 カメラ１の画素Aのデプスをカメラ２から算出する例
 カメラ１の焦点から画素Aへの視線（エピポーラ線）上をカメラ
２の画像から探索し、最も類似したテクスチャを求める。
カメラ１ カメラ2
Aのエピポーラ線
A
d
d 距離
類似度

23. Direct法: 3D Map生成
23
 各カメラから求めたデプスマップを統合して三次元モデ
ルを生成

24. Visual SLAMの研究例
 [Uchiyama2015] Uchiyama, H.,Taketomi,T., Ikeda, S., & Monte
Lima, J. P. S., "AbecedaryTracking and Mapping: a Toolkit for
Tracking Competitions," Proceedings of the 14th IEEE
International Symposium on Mixed and Augmented Reality,
pp.198-199, 2015.
 [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: Dense Tracking and Mapping in Real-Time.
In International Conference on ComputerVision.
 [Engel2014]Engel, J., Schops,T., & Cremers, D. (2014). LSD-
SLAM: Large-Scale Direct monocular SLAM. In European
Conference on ComputerVision
24

25. Visual SLAMの研究例
 [Mur-Artal2015]Mur-Artal, R., Montiel, J. M. M., & Tardos, J. D.
(2015). ORB-SLAM:AVersatile 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 :
Deep Tracking and Mapping. In European Conference on
ComputerVision.
25

26. [Uchiyama2015] ATAM
26
 SLAM初学者用に開発されたオープンソースToolkit
 可読性が高く、改変が容易
 https://github.com/CVfAR/ATAM

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

28. [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推定
28

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

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

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

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

33. 33
[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全
体を最適化

34. 34
[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

35. [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））
35

36. 36
[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
共有メモリ

37. [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間の相似変換を伝播
37

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

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

40. [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カメラからのキーポイント取得
40

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

42. [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
42

43. [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を用いて深度を補正
43

44. [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
44

45. 45
[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 深度
繰り返し処理で徐々
にサンプリング深度
を絞り込む

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

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

48. まとめ
48
 未知な環境下でのカメラの位置姿勢推定にはVisual
SLAMという技術を用いる
 SLAMは位置姿勢を推定するLocalizationと、地図を作成
するMappingの２つの処理が必要
 Visual SLAMの重要な技術として、キーポイントを検出し、
キーポイントの三次元座標とカメラの位置姿勢を同時に
推定する、バンドル調整という方法が多く用いられる
 Mapを密に求めたい場合は、画素の値を直接マッチング
させるDirect法というアプローチが取られる