![[基礎知識]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)
バイアス＝ランダムウォーク](https://image.slidesharecdn.com/20190825vins-mono-190825040310/85/20190825-vins-mono-7-320.jpg)
![[基礎知識]バンドル調整
13
1. 三次元点群とカメラパラメータの初期値を設定する
画像から見つけた点の三
次元座標の初期値カメラの位置と焦点距離
の初期値](https://image.slidesharecdn.com/20190825vins-mono-190825040310/85/20190825-vins-mono-8-320.jpg)
![[基礎知識]バンドル調整
14
2. 三次元点群を各カメラの画像平面へ投影](https://image.slidesharecdn.com/20190825vins-mono-190825040310/85/20190825-vins-mono-9-320.jpg)
![[基礎知識]バンドル調整
15
3. 投影された点と観測した点との誤差の総和を算出
投影された点
観測点
誤差](https://image.slidesharecdn.com/20190825vins-mono-190825040310/85/20190825-vins-mono-10-320.jpg)
![[基礎知識]バンドル調整
16
4. 誤差が小さくなる方向へ特徴点の三次元座標とカメラ
パラメータを調整 （収束するまで２から繰り返す）](https://image.slidesharecdn.com/20190825vins-mono-190825040310/85/20190825-vins-mono-11-320.jpg)
![[基礎知識]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に使用](https://image.slidesharecdn.com/20190825vins-mono-190825040310/85/20190825-vins-mono-12-320.jpg)
![SSII2021 [TS1] Visual SLAM ～カメラ幾何の基礎から最近の技術動向まで～](https://cdn.slidesharecdn.com/ss_thumbnails/ts1-01-210607042113-thumbnail.jpg?width=560&fit=bounds)
![[DL輪読会]NeRF: Representing Scenes as Neural Radiance Fields for View Synthesis](https://cdn.slidesharecdn.com/ss_thumbnails/nerfdlseminar1-200327021512-thumbnail.jpg?width=560&fit=bounds)
Ad
More Related Content
What's hot (20)
Similar to 20190825 vins mono (20)
![[CV勉強会]Active Object Localization with Deep Reinfocement Learning](https://cdn.slidesharecdn.com/ss_thumbnails/20160204objectdetectionrl-160206032348-thumbnail.jpg?width=560&fit=bounds)
Ad
More from Takuya Minagawa (20)
Ad
20190825 vins mono
- 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) バイアス＝ランダムウォーク
- 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
- 19. Measurement Preprocessing (Camera) 24 カメラで取得したデータに対する前処理 KLTでキーポイント追跡 Good Feature to Trackで新規キーポイントを追加し、100- 300程度となり、かつ画像全体で一様となるように 基礎行列およびRANSACを用いて幾何学的に整合性の ない点を除去 以下の条件を満たす画像をキーフレームとする ジャイロスコープを使用し、画像を平行化 前キーフレームとの平均視差が閾値以上
- 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)
- 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内のフレームのうち、新しい数フレーム分の姿勢 と速度に対してのみ最適化
- 42. Relocalization (Loop Detection) 47 最新キーフレームをクエ リーとしてPose Graph内 を検索し、ループを検出 （赤いライン）
- 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内キーフレームにマッチした特徴点の再投影誤差
- 47. Relocalization (Loop Detection) 52 Window内の古い キーフレームが周 辺化される時に Pose Graphへキー フレーム追加
- 48. Relocalization (Loop Detection) 53 Pose Graphへ追 加したキーフレー ムと過去のキーフ レーム間にループ が存在する場合、 ループ内の位置 姿勢を別スレッド 上で4DoFで最適 化（Global Pose Optimization）
- 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 ラボの席から、屋内を一周後会談を降りて、屋外をさらに 一周し、また屋内の同じ席に戻ってきたケース
- 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)式の第２，３項から今までの古いキーフレームの情報を分離し たものが第１項ではないかと予想