Mais conteúdo relacionado Semelhante a 20190825 vins mono (20) Mais de Takuya Minagawa (18) 20190825 vins mono2. 自己紹介
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
5. Visual Inertial SLAM
VI-SLAMはIMUを用いて、Visual SLAMの精度を高める手法
大きくフィルタベースの手法と最適化ベースの手法に分け
られる
フィルタベース
Kalman FilterやParticle Filterなどを使用して、IMU/カメラPose
の確率遷移を計算
最適化ベース
バンドル調整などを使用して、誤差の最小化を行うことで、カメ
ラ/IMUのPoseを推定
10
VINS-Mono
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に使用
22. Measurement Preprocessing (IMU)
27
IMUで取得したデータに対する前処理
IMUはカメラよりも周波数が高いため、フレームとフレームの
間に取得したセンサーデータを事前に積分
位置、速度、回転について算出
𝑏 𝑘における初期姿勢が変化しても 𝑏 𝑘+1の姿勢へ容易に反映
ෝ𝜶𝑖+1
𝑏 𝑘
= ෝ𝜶𝑖
𝑏 𝑘
+ 𝜷𝒊
𝑏 𝑘
𝛿𝑡 +
1
2
𝐑 ෝ𝜸𝒊
𝑏 𝑘
ො𝐚𝑖 − 𝐛a 𝑖
𝛿𝑡2
位置に関する積分項(ෝ𝜶0
𝑏 𝑘
= 𝟎)
速度に関する積分項(𝜷0
𝑏 𝑘
= 𝟎)
回転に関する積分項(ෝ𝜸0
𝑏 𝑘
= 単位クォータニオン)
𝜷𝑖+1
𝑏 𝑘
= 𝜷𝒊
𝑏 𝑘
+ 𝐑 ෝ𝜸𝒊
𝑏 𝑘
ො𝐚𝑖 − 𝐛a 𝑖
𝛿𝑡
ෝ𝜸𝑖+1
𝑏 𝑘
= ෝ𝜸𝒊
𝑏 𝑘
⨂
1
1
2
ෝ𝝎𝑖 − 𝐛 𝜔 𝑖
𝛿𝑡
(7)
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)
位置 速度 向き
(クォータニオン)
加速度
バイアス
角速度
バイアス
各特徴点の深度の逆数
31. Tightly-Coupled Monocular VIO
36
以下の式によりパラメータ𝝌を最適化(バンドル
調整)
min
𝝌 ൝
ൡ
𝒓 𝑝 − 𝐇 𝒑 𝝌
2
+
𝑘∈ℬ
𝒓ℬ ො𝐳 𝑏 𝑘+1
𝑏 𝑘
, 𝝌
𝐏 𝑏 𝑘+1
𝑏 𝑘
2
+
𝑙,𝑗 ∈𝐶
𝜌 𝒓 𝐶 ො𝐳𝑙
𝑐 𝑗
, 𝝌
𝐏𝑙
𝑐 𝑗
2
ℬ:IMU測定の集合
𝐶:SlidingWindow内で2つ以上観測される特徴点
(22)
IMUに関する残差
画像に関する残差
事前情報に対する残差
32. Tightly-Coupled Monocular VIO (IMU
Measurement Residual)
37
以下の式によりパラメータ𝝌を最適化(バンドル
調整)
min
𝝌 ൝
ൡ
𝒓 𝑝 − 𝐇 𝒑 𝝌
2
+
𝑘∈ℬ
𝒓ℬ ො𝐳 𝑏 𝑘+1
𝑏 𝑘
, 𝝌
𝐏 𝑏 𝑘+1
𝑏 𝑘
2
+
𝑙,𝑗 ∈𝐶
𝜌 𝒓 𝐶 ො𝐳𝑙
𝑐 𝑗
, 𝝌
𝐏𝑙
𝑐 𝑗
2
ℬ:IMU測定の集合
𝐶:SlidingWindow内で2つ以上観測される特徴点
(22)
IMUに関する残差
33. Tightly-Coupled Monocular VIO (IMU
Measurement Residual)
38
フレームkとk+1の間で積分したIMU
の測定結果と、2枚の姿勢変化に対
し、その残差𝒓ℬを算出。
𝑏 𝑘 𝑏 𝑘+1
k番目のフレームを
取得した時刻
k+1番目のフレーム
を取得した時刻
この間のIMUデータを積分
𝑖番目のIMU
データ
𝛿𝑡
34. Tightly-Coupled Monocular VIO (Visual
Measurement Residual)
39
以下の式によりパラメータ𝝌を最適化(バンドル
調整)
min
𝝌 ൝
ൡ
𝒓 𝑝 − 𝐇 𝒑 𝝌
2
+
𝑘∈ℬ
𝒓ℬ ො𝐳 𝑏 𝑘+1
𝑏 𝑘
, 𝝌
𝐏 𝑏 𝑘+1
𝑏 𝑘
2
+
𝑙,𝑗 ∈𝐶
𝜌 𝒓 𝐶 ො𝐳𝑙
𝑐 𝑗
, 𝝌
𝐏𝑙
𝑐 𝑗
2
ℬ:IMU測定の集合
𝐶:SlidingWindow内で2つ以上観測される特徴点
(22)
画像に関する残差
35. Tightly-Coupled Monocular VIO (Visual
Measurement Residual)
40
VINS-Monoでは魚眼カメラ等でも使用できるよう再投影
誤差𝒓 𝐶をカメラ座標系の原点を中心とした単位球上で計
算
𝒋番目の画像𝑐𝑗上の特徴点𝒍への光線
𝒊番目の画像𝑐𝑖上の特
徴点𝒍 を画像𝑐𝑗へパラ
メータ𝝌を用いて変換し、
接平面上へ投影
51. Global Pose Graph Optimization
Pose Graph全体が整合するように独立したスレッドで最
適化。
Pose Graphのノード数には上限を設け、増えすぎたとき
はダウンサンプル。
56
52. Global Pose Graph Optimization
57
Sliding Window内の古いキーフレームが周辺化され、
Pose Graphに追加されるとき、2種類のエッジを割り当
て
1. Sequential Edge
連続したキーフレームの場合
2. Loop Closure Edge
RelocalizationにおいてLoopが見つかった場合
エッジには、2つのキーフレームの相対位置ෝ𝐩𝑖𝑗
𝑖
/角
度 𝜓𝑖𝑗を割り当て
重力方向は既知なので4自由度
53. Global Pose Graph Optimization
58
各キーフレームの位置 𝐩𝑖
𝑤
およびYaw角 𝜓𝑖 を、
エッジにおける遷移ෝ𝐩𝑖𝑗
𝑖
、 𝜓𝑖𝑗とのグラフ全体
での残差が最小となるように最適化
min
𝐩,𝜓
𝑖,𝑗 ∈𝑆
𝐫𝑖,𝑗
2
+
𝑖,𝑗 ∈ℒ
𝜌 𝐫𝑖,𝑗
2
(29)
Sequential Edges
Loop Closure Edges
Huber norm
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)式の第2,3項から今までの古いキーフレームの情報を分離し
たものが第1項ではないかと予想