2019/08/25コンピュータビジョン勉強会＠関東「有名論文読み会２」発表資料です。

1. 第54回CV勉強会「有名論文読み会２」
VINS-Mono解説
2019年8月25日 takmin
1

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

3. 本日紹介する論文：VINS-Mono
8
 Qin,T., Li, P., & Shen, S. (2018).VINS-Mono:A Robust and
Versatile MonocularVisual-Inertial State Estimator. IEEE
Transactions on Robotics, 34(4), 1004–1020.
 カメラとIMUを用いたSLAM (Visual Inertial SLAM)
 コード： https://github.com/HKUST-Aerial-Robotics/VINS-Mono
 動画： https://www.youtube.com/watch?v=g_wN0Nt0VAU
 VINS-Fusionというステレオカメラに対応した拡張版
 https://github.com/HKUST-Aerial-Robotics/VINS-Fusion

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

5. Visual Inertial SLAM
VI-SLAMはIMUを用いて、Visual SLAMの精度を高める手法
大きくフィルタベースの手法と最適化ベースの手法に分け
られる
 フィルタベース
 Kalman FilterやParticle Filterなどを使用して、IMU/カメラPose
の確率遷移を計算
 最適化ベース
 バンドル調整などを使用して、誤差の最小化を行うことで、カメ
ラ/IMUのPoseを推定
10
VINS-Mono

6. Visual Inertial SLAM
11
VINS-Monoの特徴：
 ロバストな初期化手法
 カメラとIMUを統合した最適化
 オンラインでのループ検出と補正
 4自由度のポーズグラフ全体最適化
 リアルタイム（ドローン、モバイルAR）

7. [基礎知識]Inertial Measurement Unit (IMU)
 ジャイロスコープ（角速度）
 ෝ𝝎 𝑡 = 𝝎 𝑡 + 𝐛 𝜔 𝑡
+ 𝐧 𝜔
 加速度計（加速度＋重力）
 ො𝐚 𝑡 = 𝐚 𝑡 + 𝐛a 𝑡
+ 𝐑 𝑤
𝑡
𝐠 𝑤
+ 𝐧a
 ノイズ / バイアス
 𝐧 𝜔~𝑁 𝟎, 𝝈 𝜔
2
 𝐧a~𝑁 𝟎, 𝝈a
2
 ሶ𝐛 𝜔 𝑡
= 𝐧b 𝜔
~𝑁 𝟎, 𝝈b 𝜔
2
 ሶ𝐛a 𝑡
= 𝐧ba
~𝑁 𝟎, 𝝈ba
2
測定結果 真値 バイアス ノイズ
測定結果 真値 バイアス ノイズ重力向き
[https://jp.mathworks.com/help/fusion/
gs/model-imu-gps-and-insgps.html]より
転載
12
(1)
(1)
(2)
バイアス＝ランダムウォーク

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

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

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

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

12. [基礎知識]Pose Graph
17
Mur-Artal, R., Montiel, J. M. M., & Tardos, J. D.
(2015). ORB-SLAM:AVersatile and Accurate
Monocular SLAM System. IEEETransactions on
Robotics, 31(5),
 共通の特徴点を持
つキーフレーム同
士をつなげて作成
するグラフ
 ノードは回転と位置
の情報を持つ
 Loop Closingに使用

13. 参考：
18
Visual SLAM概観
 https://www.slideshare.net/takmin/20190307-
visualslam-summary
 Visual SLAM、RGBD SLAM、Visual Inertial SLAM
の代表的手法について解説した資料

14. VINS-Mono: Overview
19
 指定数の画像フレーム（= SlidingWindow）内から特徴点
を追跡し、IMUのデータを統合して非線形最適化を行うこ
とによりセンサーの姿勢を推定
 Loop Closureによって推定結果を補正

15. 処理の流れ
20

16. 処理の流れ
21
画像およびIMUか
らデータを取得し、
前処理
画像特徴点の
検出と追跡
初期Poseとパ
ラメータ推定
画像の再投影誤差
とIMUの予測誤差を
元にPose最適化
IMUの結果
を積分
Pose Graphを最適化

17. Measurement Preprocessing
 カメラとIMUからデータを取得し、前処理。
22

18. Measurement Preprocessing
 カメラとIMUからデータを取得し、前処理。
23

19. Measurement Preprocessing (Camera)
24
カメラで取得したデータに対する前処理
 KLTでキーポイント追跡
 Good Feature to Trackで新規キーポイントを追加し、100-
300程度となり、かつ画像全体で一様となるように
 基礎行列およびRANSACを用いて幾何学的に整合性の
ない点を除去
 以下の条件を満たす画像をキーフレームとする
 ジャイロスコープを使用し、画像を平行化
 前キーフレームとの平均視差が閾値以上

20. Measurement Preprocessing
 カメラとIMUからデータを取得し、前処理。
25

21. Measurement Preprocessing (IMU)
26
IMUで取得したデータに対する前処理
 IMUはカメラよりも周波数が高いため、フレームとフレームの
間に取得したセンサーデータを事前に積分
 位置、速度、回転について算出
 𝑏 𝑘における初期姿勢が変化しても 𝑏 𝑘+1の姿勢へ容易に反映
𝑏 𝑘 𝑏 𝑘+1
k番目のフレームを
取得した時刻
k+1番目のフレーム
を取得した時刻
この間のIMUデータを積分
𝑖番目のIMU
データ
𝛿𝑡

22. Measurement Preprocessing (IMU)
27
IMUで取得したデータに対する前処理
 IMUはカメラよりも周波数が高いため、フレームとフレームの
間に取得したセンサーデータを事前に積分
 位置、速度、回転について算出
 𝑏 𝑘における初期姿勢が変化しても 𝑏 𝑘+1の姿勢へ容易に反映
ෝ𝜶𝑖+1
𝑏 𝑘
= ෝ𝜶𝑖
𝑏 𝑘
+ ෡𝜷𝒊
𝑏 𝑘
𝛿𝑡 +
1
2
𝐑 ෝ𝜸𝒊
𝑏 𝑘
ො𝐚𝑖 − 𝐛a 𝑖
𝛿𝑡2
位置に関する積分項(ෝ𝜶0
𝑏 𝑘
= 𝟎)
速度に関する積分項(෡𝜷0
𝑏 𝑘
= 𝟎)
回転に関する積分項(ෝ𝜸0
𝑏 𝑘
= 単位クォータニオン)
෡𝜷𝑖+1
𝑏 𝑘
= ෡𝜷𝒊
𝑏 𝑘
+ 𝐑 ෝ𝜸𝒊
𝑏 𝑘
ො𝐚𝑖 − 𝐛a 𝑖
𝛿𝑡
ෝ𝜸𝑖+1
𝑏 𝑘
= ෝ𝜸𝒊
𝑏 𝑘
⨂
1
1
2
ෝ𝝎𝑖 − 𝐛 𝜔 𝑖
𝛿𝑡
(7)

23. Initialization
 IMUのデータと画像のデータを整合させることで、スケー
ルやバイアスなどの値を初期化
28

24. Initialization
29
Structure-from-MotionのスケールやIMUのバイアスの初期
値を算出
 Pre-IntegrationされたIMUの軌跡とSｆMで取得したカメラ
の軌跡を一致させることで各パラメータを初期化

25. Initialization
30
Structure-from-MotionのスケールやIMUのバイアスの初期
値を算出
 SfMで求めた画像間の回転変化とIMUで求めた回転との
差が最小となるようにジャイロスコープのバイアス𝐛 𝜔を
算出

26. Initialization
31
Structure-from-MotionのスケールやIMUのバイアスの初期
値を算出
 SfMで求めた画像間の回転変化とIMUで求めた回転との
差が最小となるようにジャイロスコープのバイアス𝐛 𝜔を
算出
 SfMで求めたカメラ位置とIMUから求めた位置と速度の
積分値が整合するようにパラメータ𝝌𝐼（速度、重力、ス
ケール）を算出

27. Initialization
32
Structure-from-MotionのスケールやIMUのバイアスの初期
値を算出
 SfMで求めた画像間の回転変化とIMUで求めた回転との
差が最小となるようにジャイロスコープのバイアス𝐛 𝜔を
算出
 SfMで求めたカメラ位置とIMUから求めた位置と速度の
積分値が整合するようにパラメータ𝝌𝐼（速度、重力、ス
ケール）を算出
 重力ベクトル𝐠 𝑐0を重力加速度を既知として再算出し、z
軸と重力ベクトルが一致するように世界座標系を定義

28. Local Visual-inertial Odometry with
Relocalization
 画像とIMUの情報を元に、バンドル調整によってフレーム
ごとのカメラ姿勢を算出
 過去に取得したキーフレームとのマッチングを行うことで
ループを検出し、Window内のキーフレームの姿勢を補
正
33

29. Local Visual-inertial Odometry with
Relocalization
34
 SLAMで求めるパラメータを以下のように定義
𝝌 = 𝐱0, 𝐱1, ⋯ , 𝐱 𝑛, 𝐱 𝑐
𝑏, 𝜆0, 𝜆1, ⋯ , 𝜆 𝑚
 Window内のk番目のキーフレームにおけるIMUの
状態
𝐱 𝑘 = 𝐩 𝑏 𝑘
𝑤
, 𝐯 𝑏 𝑘
𝑤
, 𝐪 𝑏 𝑘
𝑤
, 𝐛a, 𝐛ω
 body(IMU)座標でのカメラ姿勢
𝐱 𝑐
𝑏 = 𝐩 𝑐
𝑏, 𝐪 𝑐
𝑏
(22)
位置 速度 向き
(クォータニオン)
加速度
バイアス
角速度
バイアス
各特徴点の深度の逆数

30. Tightly-Coupled Monocular VIO
 画像とIMUの情報を元に、バンドル調整によってフレーム
ごとのカメラ姿勢を算出
35

31. Tightly-Coupled Monocular VIO
36
 以下の式によりパラメータ𝝌を最適化（バンドル
調整）
min
𝝌 ൝
ൡ
𝒓 𝑝 − 𝐇 𝒑 𝝌
2
+ ෍
𝑘∈ℬ
𝒓ℬ ො𝐳 𝑏 𝑘+1
𝑏 𝑘
, 𝝌
𝐏 𝑏 𝑘+1
𝑏 𝑘
2
+ ෍
𝑙,𝑗 ∈𝐶
𝜌 𝒓 𝐶 ො𝐳𝑙
𝑐 𝑗
, 𝝌
𝐏𝑙
𝑐 𝑗
2
ℬ：IMU測定の集合
𝐶：SlidingWindow内で２つ以上観測される特徴点
(22)
IMUに関する残差
画像に関する残差
事前情報に対する残差

32. Tightly-Coupled Monocular VIO (IMU
Measurement Residual)
37
 以下の式によりパラメータ𝝌を最適化（バンドル
調整）
min
𝝌 ൝
ൡ
𝒓 𝑝 − 𝐇 𝒑 𝝌
2
+ ෍
𝑘∈ℬ
𝒓ℬ ො𝐳 𝑏 𝑘+1
𝑏 𝑘
, 𝝌
𝐏 𝑏 𝑘+1
𝑏 𝑘
2
+ ෍
𝑙,𝑗 ∈𝐶
𝜌 𝒓 𝐶 ො𝐳𝑙
𝑐 𝑗
, 𝝌
𝐏𝑙
𝑐 𝑗
2
ℬ：IMU測定の集合
𝐶：SlidingWindow内で２つ以上観測される特徴点
(22)
IMUに関する残差

33. Tightly-Coupled Monocular VIO (IMU
Measurement Residual)
38
 フレームkとk+1の間で積分したIMU
の測定結果と、２枚の姿勢変化に対
し、その残差𝒓ℬを算出。
𝑏 𝑘 𝑏 𝑘+1
k番目のフレームを
取得した時刻
k+1番目のフレーム
を取得した時刻
この間のIMUデータを積分
𝑖番目のIMU
データ
𝛿𝑡

34. Tightly-Coupled Monocular VIO (Visual
Measurement Residual)
39
 以下の式によりパラメータ𝝌を最適化（バンドル
調整）
min
𝝌 ൝
ൡ
𝒓 𝑝 − 𝐇 𝒑 𝝌
2
+ ෍
𝑘∈ℬ
𝒓ℬ ො𝐳 𝑏 𝑘+1
𝑏 𝑘
, 𝝌
𝐏 𝑏 𝑘+1
𝑏 𝑘
2
+ ෍
𝑙,𝑗 ∈𝐶
𝜌 𝒓 𝐶 ො𝐳𝑙
𝑐 𝑗
, 𝝌
𝐏𝑙
𝑐 𝑗
2
ℬ：IMU測定の集合
𝐶：SlidingWindow内で２つ以上観測される特徴点
(22)
画像に関する残差

35. Tightly-Coupled Monocular VIO (Visual
Measurement Residual)
40
 VINS-Monoでは魚眼カメラ等でも使用できるよう再投影
誤差𝒓 𝐶をカメラ座標系の原点を中心とした単位球上で計
算
𝒋番目の画像𝑐𝑗上の特徴点𝒍への光線
𝒊番目の画像𝑐𝑖上の特
徴点𝒍 を画像𝑐𝑗へパラ
メータ𝝌を用いて変換し、
接平面上へ投影

36. Tightly-Coupled Monocular VIO
(Marginalization)
41
 以下の式によりパラメータ𝝌を最適化（バンドル
調整）
min
𝝌 ൝
ൡ
𝒓 𝑝 − 𝐇 𝒑 𝝌
2
+ ෍
𝑘∈ℬ
𝒓ℬ ො𝐳 𝑏 𝑘+1
𝑏 𝑘
, 𝝌
𝐏 𝑏 𝑘+1
𝑏 𝑘
2
+ ෍
𝑙,𝑗 ∈𝐶
𝜌 𝒓 𝐶 ො𝐳𝑙
𝑐 𝑗
, 𝝌
𝐏𝑙
𝑐 𝑗
2
ℬ：IMU測定の集合
𝐶：SlidingWindow内で２つ以上観測される特徴点
(22)
事前情報に対する残差

37. Tightly-Coupled Monocular VIO
(Marginalization)
42
新しくフレームが加わった際、以下の処理を行う
 １つ前のフレームがキーフレームの時、Window内の最も古いフレーム
のパラメータを周辺化（Schur Complement）して事前分布とする
 １つ前のフレームがキーフレームではない場合、そのフレームを破棄

38. Tightly-Coupled Monocular VIO (Motion-only
BA)
43
 モバイルデバイスなど、計算能力の低い機器の場合、
Window内のフレームのうち、新しい数フレーム分の姿勢
と速度に対してのみ最適化

39. Relocalization
 VIOのずれを補正するために、過去に取得したキーフ
レームとのマッチングを行うことでループを検出し、
Window内のキーフレームの姿勢を補正
44

40. Relocalization (Loop Detection)
45

41. Relocalization (Loop Detection)
46
VIOのみで位置/
姿勢推定（青）

42. Relocalization (Loop Detection)
47
最新キーフレームをクエ
リーとしてPose Graph内
を検索し、ループを検出
（赤いライン）

43. Relocalization (Loop Detection)
48
マッチしたキーフ
レームを元に位
置姿勢を更新
（青い個所のみ）

44. Relocalization (Loop Detection)
49
特徴点がマッチした複数の過去フレームを制約に加えて姿
勢を補正

45. Relocalization (Tightly-Coupled
Relocalization)
50
 Loopが検出された場合、Window内のキーフレームとマッチン
グした特徴点を用いて最適化（バンドル調整）
min
𝝌 ൝
ൡ
𝒓 𝑝 − 𝐇 𝒑 𝝌
2
+ ෍
𝑘∈ℬ
𝒓ℬ ො𝐳 𝑏 𝑘+1
𝑏 𝑘
, 𝝌
𝐏 𝑏 𝑘+1
𝑏 𝑘
2
+ ෍
𝑙,𝑗 ∈𝐶
𝜌 𝒓 𝐶 ො𝐳𝑙
𝑐 𝑗
, 𝝌
𝐏𝑙
𝑐 𝑗
2
+ ෍
𝑙,𝑣 ∈ℒ
𝜌 𝒓 𝐶 ො𝐳𝑙
𝑣
, 𝝌, ෝ𝒒 𝑣
𝑤
, ෝ𝒑 𝑣
𝑤
𝐏𝑙
𝑐 𝑣
2
ℒ：Loop DetectionでマッチしたPose Graph内の特徴点セット
(25)
IMUに関する残差
画像に関する残差
事前情報に対する残差
VIOのコスト関数(22)に新たに追加した項
Pose Graph内キーフレームにマッチした特徴点の再投影誤差

46. Relocalization (Loop Detection)
51

47. Relocalization (Loop Detection)
52
Window内の古い
キーフレームが周
辺化される時に
Pose Graphへキー
フレーム追加

48. Relocalization (Loop Detection)
53
Pose Graphへ追
加したキーフレー
ムと過去のキーフ
レーム間にループ
が存在する場合、
ループ内の位置
姿勢を別スレッド
上で4DoFで最適
化（Global Pose
Optimization）

49. Relocalization (Loop Detection)
54
以後のRelocalization
は最適化されたPose
Graphに対して行う

50. Relocalization (Feature Retrieval)
55
ループが検出されると、Sliding
Window内の画像とマッチした
キーフレーム間で特徴点マッチ
ングが行われ、Window内の
キーフレームの位置姿勢が更
新されます
a. BRIEF特徴量でマッチングし
た結果
b. RANSACでF行列を求め、
その過程で出てきたOutlier
を除去
c. 既知の3D点と特徴点との
マッチングをRANSACとPnP
で行い、Outlierを除去

51. Global Pose Graph Optimization
 Pose Graph全体が整合するように独立したスレッドで最
適化。
 Pose Graphのノード数には上限を設け、増えすぎたとき
はダウンサンプル。
56

52. Global Pose Graph Optimization
57
Sliding Window内の古いキーフレームが周辺化され、
Pose Graphに追加されるとき、２種類のエッジを割り当
て
1. Sequential Edge
 連続したキーフレームの場合
2. Loop Closure Edge
 RelocalizationにおいてLoopが見つかった場合
 エッジには、２つのキーフレームの相対位置ෝ𝐩𝑖𝑗
𝑖
/角
度 ෠𝜓𝑖𝑗を割り当て
 重力方向は既知なので4自由度

53. Global Pose Graph Optimization
58
各キーフレームの位置 𝐩𝑖
𝑤
およびYaw角 𝜓𝑖 を、
エッジにおける遷移ෝ𝐩𝑖𝑗
𝑖
、 ෠𝜓𝑖𝑗とのグラフ全体
での残差が最小となるように最適化
min
𝐩,𝜓
෍
𝑖,𝑗 ∈𝑆
𝐫𝑖,𝑗
2
+ ෍
𝑖,𝑗 ∈ℒ
𝜌 𝐫𝑖,𝑗
2
(29)
Sequential Edges
Loop Closure Edges
Huber norm

54. Experimental Results (EuRoC)
 Intel i7-4790 CPU (3.60GHz)でリアルタイム
 EuRoC MAVVisual Inertial Datasetで評価
59

55. Experimental Results (Indoor)
60
 20Hzのカメラ、100HzのIMUでラボ内を歩いた結果

56. Experimental Results (Large Scale)
61
 ラボの席から、屋内を一周後会談を降りて、屋外をさらに
一周し、また屋内の同じ席に戻ってきたケース

57. Experimental Results (Drone)
62

58. まとめ
63
VINS-Mono
 単眼Visual Inertial SLAM
 カメラとIMUを統合した非線形最適化を解くことでキーフ
レームの位置と姿勢を推定
 IMUの持つ加速度と角速度の情報を元に、位置と速度、
および角度をあらかじめ積分しておくことで、各キーフ
レームにおける最適化を高速化
 オンラインでのループ検出と補正
 4自由度のポーズグラフ全体最適化

59. Local Visual-inertial Odometry with
Relocalization (Marginalization)
65
周辺化（Marginalization）
 一般に同時分布のパラメータを積分によって減らすことを指す
 この論文ではSliding Window内の最も古いキーフレームのパ
ラメータ𝐱 𝑘、 𝜆𝑙をSchur complementという手法で周辺化するこ
とで、それに紐づく測定結果を事前分布とするとあるが、詳細
の記載なし
 Schur complementはガウス分布を周辺化した際の平均/共分散行
列を簡単に計算する方法
 (22)式の𝒓 𝑝、 𝐇 𝒑は周辺化によって求まるとあるが、詳細の記
載なし
 二乗誤差最小化はガウス分布に対する最尤推定とみなせるので、
(22)式の第２，３項から今までの古いキーフレームの情報を分離し
たものが第１項ではないかと予想