Recommended
More Related Content
What's hot
What's hot (20)
Similar to 20190825 vins mono
Similar to 20190825 vins mono (20)
More from Takuya Minagawa
More from Takuya Minagawa (19)
Recently uploaded
Recently uploaded (8)
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)式の第２，３項から今までの古いキーフレームの情報を分離し たものが第１項ではないかと予想