vinsシリーズの概要

序章

vins-mono+vins-fusiuon のいくつかの主要な問題の概要

1. vins-mono アルゴリズム フレームワークの概要

アルゴリズム フレームワークは主に、データ前処理、システム初期化、スライディング ウィンドウに基づく VIO プロセス、およびループバック検出の 4 つのモジュールに分かれています。

• データ前処理モジュールには主に画像ヒストグラム等化、KLT特徴点抽出、歪み補正処理、オプティカルフロー追跡が含まれており、IMU部分は主に事前統合演算を実行します。

• システムの初期化では、最初に純粋なビジュアル sfm が実行され、主にエピポーラ制約に基づいて、スケール情報とt を実行し、一連の 3D 点を三角形化します。最後に、画像の他のフレームに対応するポーズが pnp によって解決されます。

• 視覚的な初期化が完了すると、カメラと IMU 間の回転外部パラメータとジャイロスコープのバイアス情報に対して疎結合に基づくオンライン キャリブレーションが実行され、画像の各フレームに対応する速度、スケール情報、重力加速度が計算されます。重力加速度は、単位空間の接平面内で 2 つの単位直交基底式に分解され、最適解を得るために残差関数に戻されます。

• スライディング ウィンドウに基づく VIO プロセスは通常の VIO プロセスであり、残差関数は周辺化によって得られる事前残差、視覚的再投影残差、および IMU 残差の 3 つの部分で構成されます。

• ループ検出モジュールは主に DBOW2 バッグ オブ ワード ライブラリに基づいて実装されており、キー フレームごとに 500 個の特徴点が抽出され、記述子が計算されてバッグ オブ ワード ベクトルに変換されます。ループ キー フレームはバッグを通じて検出されます。の言葉ベクトルのセット。特徴マッチング関係を確立した後, エッセンシャル行列を繰り返し解き, RANSAC に基づいて PNp を順番に実行します. 目的は外れ値をスクリーニングすることです. インライアの数が要件を満たしている場合, ループが検出され, ループの残差が計算されます残差関数に追加 4 DOF ポーズ グラフの最適化。
  

2. vins-fusion アルゴリズム フレームワークの概要

Vins-fusion は主に、vins-mono に基づいて双眼、双眼+IMU、GPS のサポートを追加します。

GPS 部分は主に座標系が統一されており、初期フレームの SLAM 姿勢を GPS 座標に合わせることで相対姿勢を絶対姿勢に変換します。同時に、GPS の絶対姿勢情報と SLAM 座標と GPS 座標の間の相対変換が残差関数に追加され、ソリューションが最適化されます。

3. 画像情報の前処理

センサー前処理モジュールは、SLAM フロントエンドに相当します。ビジョンモジュールの場合、主に画像前処理 (ヒストグラム等化、あまり実用的ではありません)、KLT コーナー検出 (Shi-Tomas コーナー) と歪み補正処理、LK ピラミッド オプティカル フロー トラッキング、その他の部分が含まれます。

4. オプティカルフロー追跡プロジェクト

オプティカル フロー トラッキングの 3 つの前提、LK オプティカル フロー アルゴリズムの原理、ピラミッド オプティカル フロー アルゴリズムの原理、KLT オプティカル フロー トラッキングの原理、および KLT コーナー検出の原理が含まれます。

4.1 直接法またはオプティカル フロー トラッキングの 3 つの主要な前提条件

オプティカル フロー トラッキングの主な原理は、隣接するフレーム画像間のピクセルの速度を計算することで、次のフレームのピクセルの位置を取得することです。主に次の仮定に基づいています。

• グレースケールの一貫性: 同じ空間点のピクセルのグレースケール値が各画像で固定されます。
• 連続的な小さな動き: 小さな動きは隣接するフレーム間で維持され、時間の変化によってピクセルが大幅に変化することはありません。
• 空間的一貫性: 特徴点とその周囲の近傍のピクセルはすべて同じ動きをします。

4.2 LK オプティカル フロー追跡アルゴリズムの原理

（ここでは第 14 章第 8 章を参照）
時刻 t におけるあるピクセル (x, y) について、カメラが時刻 t+1 に移動すると、グレー不変性の仮定に基づいて次の関係が得られます。

式の左側で 1 次テイラー展開が実行され、グレーの不変性に従って次の式が得られます。

最初dx/dtの とdy/dtはそれぞれ、x 軸方向と y 軸方向のピクセル点の導関数であり、それぞれ u と v として記録されます。
次に∂I/∂x、 と は∂I/∂yそれぞれ (x, y) のピクセル値の偏導関数で、次のように記録されます。 (x, y) 勾配;
上記の式は行列の形式で書くことができます。

2 つの隣接するフレーム内のピクセルの速度 (u, v) を解決するには、同じピクセル ブロック内のピクセルが空間的一貫性に従って同じ動きを持つと仮定して、特徴点ピクセルの周囲の近傍範囲を選択できます。そして、この範囲の画素が特徴点と同じ動きをしている場合、最小二乗法に従って変数(u,v)について連立方程式を解くことができる。

4.3 ピラミッドオプティカルフロー

この部分の参照リンク: https://blog.csdn.net/shandianfengfan/article/details/110944659

カメラの動きが速すぎると、隣接するフレーム画像間に大きなずれが生じますが、このとき、隣接する画像における同じ特徴点のずれ Δ_pixel は比較的小さいため、動きが小さいという強い仮定は無効になります。を反復的に解くと、局所最適に陥りやすくなります。

ピラミッド オプティカル フローの中心的なアイデアは、画像をピラミッドで処理することです。オプティカル フローを計算するときは、最初に最小のピクセル解像度を持つ最上位の画像のオプティカル フローを計算します。画像の解像度が小さいため、ピクセルの移動距離は比較的小さく、これは小さな動きに対するオプティカル フローの仮定と一致しています。ピラミッドの上部画像のピクセルの速度を解決した後、ピクセル点の速度と勾配情報を初期値として使用して、次のレイヤーの特徴点の速度と勾配情報を計算し続けます。元の画像内の特徴点の速度情報が解決されます。

ピラミッドの作成
元の画像はガウス畳み込みによってダウンサンプリングされます。

画像ピラミッド画像は通常 4 層あり、層数が多いほど追跡の精度は高くなりますが、計算量は増加します。vins-mono では 4 層のピラミッドが選択され、vins-fusion では予測のステップが追加されます。予測が成功すると、現在のモーション トラッキングの方が優れていることが示され、ピラミッドは 2 層に削減されます。左右の目はまだ4層のピラミッドです。

ピラミッド オプティカル フロー トラッキングの一般的な手順は次のとおりです。

• ピラミッドが確立されたら、まず最上位レイヤの画像に対してレイヤの初期オプティカル フローと残留オプティカル フロー ベクトルを計算します。各レイヤの初期オプティカル フローは下図に示されます
  

オプティカル フロー残差ベクトルは、基準フレームから現在のフレームへの同じピクセルの移動を表し、ピクセル ブロックのグレー レベル差を最小化することによって取得されます。初期オプティカル フロー g によって初期追跡点が決定され、残りのオプティカル フロー d が初期追跡点に基づいて繰り返し最適化されて、正確な追跡点が取得されます。

• 上記の初期オプティカル フロー計算式に従って、各レイヤの初期オプティカル フローを取得でき、その後、反復を通じて残りのオプティカル フローと正確なオプティカル フローの位置を見つけることができます。
• 元画像上のピクセルの座標が得られるまで上記の処理を繰り返します。

4.4 KLT オプティカル フロー トラッキング

中心的なアイデアは、適切に追跡できる特徴点検出方法、つまり Shi-Tomasi コーナー検出を提供し、 vins の cv の関数を直接呼び出すことですgoodFeaturesToTrack。

4.5 Harris角点

参考リンク: https://zhuanlan.zhihu.com/p/150411153
Harris コーナー検出アルゴリズムの核心は、ローカル ウィンドウを使用して画像上を移動し、グレー レベルが大幅に変化したかどうかを判断することです。ウィンドウ (グラデーション マップ上) のグレー値に大きな変化がある場合、ウィンドウが配置されている領域に角があります。

ハリスコーナーポイントは、ウィンドウ移動前後のウィンドウ内の画素の階調差を計算して画素値変化関数E(x,y)を求め、行列の固有値を計算してコーナーポイントの有無を判定します。 in E(x, y). 2 つの固有ベクトルが比較的大きい場合、ウィンドウ内にコーナー点があると考えられます。

1. ハリスコーナー検出手順:

• ウィンドウ（局所領域）が x（水平）方向と y（垂直）方向に同時に移動したとき、ウィンドウ内のピクセル値の変化 E(x,y) を計算します。
• 各ウィンドウについて、対応するコーナー応答関数 R を計算します。
• R>threshold の場合、ウィンドウがコーナー フィーチャに対応することを意味します。

2. ハリス コーナー数学モデル
数学モデルを確立する目的は、どのウィンドウがグレー値の大きな変化を引き起こすかを決定することです。ウィンドウ内の点 (x, y) の場合、そのグレー値は I(x, y) で、それが (u, v) の距離移動すると、対応するグレー値は I(x+u,y+ v) になります。I(x+u,y+v) - I(x,y)つまり、ウィンドウが移動しているときのピクセルのグレー値の変化です。

ウィンドウ内のピクセルのグレースケールの変化をより明確に判断するには、ウィンドウ内のすべてのピクセルのグレースケール値の変化を計算することを選択します。ウィンドウの中心点のピクセルがコーナー点の場合、ウィンドウの移動の前後で中心点のグレー値が非常に大きく変化するため、この点の重み係数を少し大きく設定する必要があります。グレー レベルの変化に大きく寄与します。(コーナー ポイント) 遠い点では、これらのポイントのグレー レベルの変化は比較的小さいため、重み係数は少し小さく設定されます。これは、グレー レベルの変化に対するこの点の寄与が小さい。

したがって、ここではウィンドウ関数 w(x, y) (実際にはガウス カーネル) を導入して、ウィンドウ全体のピクセル グレー値の変化を計算します。移動前後のウィンドウの濃淡値の変化をE(u,v)とすると、ウィンドウの中心ピクセルがコーナー点の場合、E(u,v)は相対的に大きくなる。

計算を簡略化するために、I(x+u,y+v)一次テイラー展開が実行されます。

対応する行列 M は次のとおりです。

3. ハリス コーナーの解決策
ハリス コーナーの数学的モデルが定義されたので、次のステップは、コーナーの座標とコーナーが存在する対応するウィンドウを見つける方法です。

(1) M 行列の固有値による判断
固有値が両方向に比較的大きい場合はコーナー、固有値が一方向に比較的大きい場合はエッジ、固有値が 1 方向に比較的大きい場合はエッジ、固有値が小さい、それは平面です。

R は、直交する 2 方向の変動成分に影響を与えない回転係数とみなすことができます。対角化後、直交する 2 方向の変化成分が抽出され、それが λ1 と λ2 (固有値) となります。

(2) R 応答による判定は、
基本的に、M 行列の固有値に基づいてスコア R を計算し、そのスコアに従って判定します: R が特定のしきい値より大きい場合、コーナー ポイントとみなされます; R が 0 より大きいただし、しきい値未満は画像ブロック、R が 0 未満はエッジです。
R は次のように計算されます。

ハリス コーナー検出の結果は、これらのスコア R を持つグレースケール画像であり、しきい値が設定され、このしきい値より大きいスコアを持つピクセルがコーナーに対応します。

4.6 シ・トマシコーナー検出

参考リンク： https: //blog.csdn.net/qq_40374812/article/details/117016214
シ・トマシコーナーポイントはハリスコーナーポイントの改良版であり、主にコーナーポイント応答値の計算方法を改良したものです。の固有値がコーナー応答値として使用され、しきい値と比較されます。

Harris コーナー検出アルゴリズムでは、コーナー応答値 R を導入し、R の値に応じてコーナーと境界の有無を判断します。

Shi-Tomasi コーナー検出アルゴリズムを最適化し、R 値の計算式を修正しました。ハリスコーナー検出アルゴリズムの安定性はα値に関係しており、αは経験値であるため、経験に基づいて設定する必要があり、最適な値を設定することは容易ではありません。

Shi-Tomasi は、コーナーポイントの安定性が実際には行列 M の小さい固有値 (つまり、λ1、λ2) に関係していることを発見しました。そのため、小さい固有値がスコアとして直接使用されます。これにより、アルファ値を調整する必要がなくなります。

そこで、Shi-Tomasi は R 値の計算式を次の形式に変更しました。R = min(λ1，λ2)

Harris と同様に、スコアが設定されたしきい値よりも大きい場合、それをコーナーとみなします。

5. vins-monoにおける特徴点歪み除去法

参考リンク：https://blog.csdn.net/qq_33733356/article/details/124356158

段階的近似法を使用する場合、基本的な考え方は、画像の端に近づくほど歪みの度合いが大きくなるということです。

歪みを含む座標が与えられた場合A'、歪みが生じる前の実際の座標を見つける必要がありますA。歪み公式によれば、実際の座標から歪みを含めた座標を計算したり、その逆も比較的簡単に計算できます。

したがって、VINS では、既知のA'座標が実際の座標とみなされB、歪んだ座標は歪公式 によって取得されますB'。移動BB'から歪みの強さ（近似値）を取得し、より近い点を取得します。次に、より近い点の推定に進みます。反復を繰り返すことで、許容範囲内で限りなく近い点を取得できます。A'BB'ACCADA

6. 特徴点の均一化

特徴点を分類する場合、特徴点を追跡する時間が長いほど安定性が高く、品質が高いと考えられます。次に、特徴点を中心として近傍を描画し、特徴点の近傍にある他の特徴点を無視します。

7. 追跡中の外れ値のスクリーニング

• オプティカル フロー トラッキングでは、トラッキングされていない特徴点が削除されます
• 画像境界の外側の点はオプティカル フロー トラッキング後に削除されます
• 次のフレームで特徴点が追跡された後、RANSAC に基づいて必須行列が計算され、エピポーラ制約の原理によって外れ値が選別されます。

8. vins-monoでのキーフレーム作成方法

主に前のキーフレームフレームとの差分と現在のトラッキング品質に基づいて判断されます

• 視差:現在のフレームと前のキー フレームには十分な回転視差と並進視差が必要です。回転視差はジャイロスコープの角速度を積分することで得られ、並進視差は一致するすべての特徴点の平均変位を計算することで得られます。
• 追跡品質:追跡されたフィーチャの数が特定のしきい値を下回っている場合、現在のフレームを新しいキーフレームとして設定します。

9. オプティカル フロー トラッキングと記述子マッチングの比較

1. 時間のかかる比較

• オプティカル フロー追跡の場合、各フレームで 100 個の特徴点の抽出が必要であると想定されます。これは、オプティカル フローは前のフレーム (N の場合) のほとんどの特徴点を直接追跡できるため、インターバル間でのみ抽出する必要があるためです。 - フレーム追跡 (100-N ) 特徴点時間とオプティカル フロー追跡時間。
• 記述子マッチングの場合、各フレームで 100 個の特徴を抽出するのにかかる時間と記述子マッチング時間を計算する必要があります。

2. パフォーマンスの比較
LK オプティカル フローは記述子マッチングよりも堅牢ですが、オクルージョンと高速移動の場合には記述子ベースの方法の方がパフォーマンスが優れています。したがって、オプティカル フロー トラッキングはフレーム間マッチングに適しており、ディスクリプタ マッチングは再配置とループ閉鎖の検出に適しています。

10. 事前統合を使用する理由

• 誤差の伝播を軽減するには、積分プロセス中にオフセットやその他の情報を計算する必要があり、積分を繰り返すと誤差が蓄積されます。
• SLAM システムでは、レーザーまたはカメラの周波数は IMU の周波数よりもはるかに小さく、バックエンドの最適化中に何度も反復されます。ポーズが調整されるたびに、2 つのフレーム間の IMU データが繰り返し再統合する必要があり、これには比較的長い時間がかかります。コンピューティング リソース。

10. センサー前処理のための IMU 事前統合

• このセクションでは、離散時間での PVQ の事前積分計算は、連続時間での IMU の積分を導出することで得られます。
• 次に、誤差カルマン フィルター法を vin 単位で解析します (つまり、誤差コスト関数は、隣接する瞬間の動きの推定値と隣接する瞬間の IMU 事前積分量の差を使用します)。
• 次に、連続時間での IMU 状態誤差から、中央積分に基づく離散時間での IMU 状態の誤差伝達を推定し、事前積分における IMU ゼロ バイアスのモデリングを分析します。
• 最後に、隣接する瞬間における事前に積分された共分散行列の伝達が推定されます。

10.1 連続時間 PVQ 統合

連続時間における pvq の積分公式は次のとおりです。

時刻 t での加速度積分処理では、まず、現時点での加速度ゼロバイアスから現在の加速度計の読み取り値を減算し、ガウス測定ノイズを減算し、時刻 t での加速度を世界座標系 ( VIO システムでは、世界座標系は重力の方向と一致している必要があり、加速度を世界座標系に転送した後、重力の影響を直接差し引くことができます); 最後に、時間 t における最終加速度が得られます。重力を差し引くことによって。

クォータニオン導出プロセスは次のように導出できます。

時間 k+1 での最終回転 q_k+1 は、時間 K での回転 q_k に時間 Δt での回転の積分を乗じることによって取得できます。ここで、w はジャイロスコープによって測定された角速度です。

10.2 連続時間における事前積分量の計算

上記の連続モーメントの積分では、基準座標系をワールド座標系からローカル座標系に変換することで、ローカル座標系での積分前の表現を取得できます。

このうち、α、β、γはそれぞれ k から k+1 までの変位、速度、回転の事前積分量です。

10.3 離散時間での事前積分

しかし、実際には、私たちが取得する測定値はすべて離散時間に基づいているため、隣接する時間の前積分を計算するときは、連続時間の前積分を離散時間の前積分に変換する必要があります。中央積分は、vins-mono アルゴリズムの離散化に使用されます。

式の添字問題では、2 つの隣接するモーメントの変位、速度、回転が事前積分中に計算されるため、すべての事前積分は時間 k での変換を基準とします。

10.4 誤差カルマンフィルター

vins級数では、最適化時の誤差関数の定義が従来(ORB-SLAM)の定義と若干異なり、k回からk+1回まで、推定に基づきk+1回目の状態(相対変位を含む) 、速度、回転、バイアス）、k+1 回と k 回の状態の差が推定相対運動として記録され、IMU pre に従って k 回から k+1 回までの相対運動の測定値を取得できます。 -積分、ビン最適化 誤差関数は、推定された相対運動と測定された相対運動 (積分前) が最小化されるときの k+1 時刻に対応する状態を解くことです。

なぜエラーカルマンフィルタリングを使用するのでしょうか? ?
(1) 4 自由度の四元数を使用して 3 自由度の回転を記述すると過剰パラメータ化が発生するため、回転ベクトルを使用して回転を直接記述します。一般に、回転ベクトルは周期的です (回転ベクトルは周期的です)。
(2) 回転の場合、誤差は 0 付近で変化することが保証され、問題が発生します。ジンバルデッドロックなどを回避できる
(3) 誤差が小さく、2次微分は無視でき、1次ヤコビアンのみを計算する

10.5 連続時間 IMU 状態エラー状態の導出

各状態に関する IMU エラーの導関数:

次の導出参考リンク: https://blog.csdn.net/weixin_45626706/article/details/118074274

変位成分に関する誤差関数の微分:

速度に関する誤差関数の導関数:

回転に関する誤差関数の微分:

バイアスに関する誤差関数の微分:

10.6 中央値積分に基づく離散時間事前積分の誤差伝播

具体的には：

離散時間角度誤差の導出

離散時間速度誤差の導出

離散時間変位誤差の導出

離散時間ゼロバイアス誤差の導出

10.7 統合前における IMU バイアスのモデリング

IMU 統合プロセスの計算にはゼロ バイアスが含まれます。これは、VINS-MONO が密結合された VIO であるため、そのゼロ バイアスも推定量です。つまり、最適化問題を解くプロセスで、最適化ごとにゼロ バイアスが変更される可能性があります。全体最適化の最低限を満たす条件を作ります。これは問題を引き起こします。あらゆる最適化には積分の再計算が必要です。

vins では、ゼロバイアスへの一次近似の方法が事前積分量の更新に使用されます。
f(x+Δx) = f(x) + J(x)Δx = 原先预积分的结果 + 相对于零偏的一阶雅克比 * 补偿量

ゼロバイアスに関する事前積分量のヤコビアンを解く

10.8 事前積分共分散行列の転送

假设a符合高斯分布,其方差为A。对a乘以系数k, 则ka的方差为k^2A
若a为向量,则其协方差为A的矩阵
若F矩阵乘以a向量,其协方差为FAF^T
若向量a的协方差矩阵为A,向量b的协方差矩阵为B,则向量a+向量b的协方差矩阵为A+B矩阵

11. VIO 初期化プロセスの概要

システムの初期化では、最初に純粋なビジュアル sfm が実行され、主にエピポーラ制約に基づいて、スケール情報とt を実行し、一連の 3D 点を三角形化します。最後に、画像の他のフレームに対応するポーズが pnp によって解決されます。

次に、各フレームに対応する回転、ジャイロスコープのバイアス、速度、スケール、重力加速度の外部パラメーターの初期化を含む視覚的慣性アライメントを実行し、最後に重力加速度を調整します。

12. VIO は変数を初期化しない

1. 並進外部パラメータ 並進
   外部パラメータの場合、加速度二重積分が必要であり、ノイズが多い このノイズベースの並進推定に基づいて取得された外部パラメータは、直接測定ほど正確ではありません IMU とカメラの間の並進を測定することで誤差外部パラメータは数ミリメートル以内で制御できます。さらに、外部パラメータは後続の非線形最適化で継続的に最適化されます。
2. 加速度計のバイアス
   加速度計のバイアスを重力推定から切り離すのは難しく、加速度計のバイアスを無視しても結果に大きな影響はありません。このアルゴリズムでは、初期バイアスが 0 に設定され、この値は非線形最適化プロセス中に継続的に更新されます。

13. IMU とカメラ間の回転外部パラメータの校正

IMU とカメラ間の回転外部パラメータは疎結合キャリブレーション アルゴリズムを使用しており、時間 k から k+1 までの間、2 つのフレーム間の回転運動 R_c は純粋な視覚的 SFM によって復元でき、IMU の測定に基づいています。 k 回から k+1 回までの積分 R_b が得られ、任意の時刻において次式が成立します。

いくつかのフレームの後、隣接するフレームのグループ間の視覚的な回転と、対応する IMU 統合前の回転を取得でき、この制約関係を通じて最小二乗問題を構築して、カメラと IMU の間の回転外部パラメータを解決できます。

14. 純粋な単眼視覚の初期化

現在のスライディング ウィンドウの画像フレームに対して純粋な単眼 SLAM を実行し、スケール情報なしでカメラのポーズと 3D ポイントを復元します。

1. スライディング ウィンドウの画像フレーム内で、十分な特徴点 (30) と十分な大きさの視差 (20x ピクセル) を持つ 2 つの画像フレームを見つけます (これが 4 番目と 10 番目のフレームであると仮定します)。

2. エピポーラ制約の原理に従って、RANSAC 反復解法に基づいて、4 番目のフレームと 10 番目のフレームの動きが計算され、(R,t)スケール情報のない 3D 空間点が復元されます。
このプロセスは ORB と同じです。 -スラム。

3. 取得した 3D ポイントに従って、スライディング ウィンドウの他のフレームのポーズ (スケールなし) が pnp によって解決され、同時により多くの 3D ポイント (スケールなし) が復元されます。

4. セレス自動導出に基づいて再投影誤差を最小限に抑え、スケールなしで最適化されたポーズを取得します

15. ジャイロバイアスの初期化

まず、時間 k における c0 参照フレームへの IMU の回転と平行移動を定義できます。

具体的な導出は以下の通りです。

k フレームと k+1 フレームの 2 つの連続したフレームについては、以前の純粋な視覚の初期化に従って、c0 参照フレームに対する IMU の姿勢の推定値とq_bk_c0、q_bk+1_c0

その後、IMU 自体が 2 つのフレーム間の回転の測定値を取得できますγ_bk+1_bkが、ジャイロスコープのバイアスの関係により、推定された姿勢の間の相対回転と事前積分によって得られた相対回転は完全に等しくなりません。 1行分のエラーモデルを定義します。

vins では、加速度計とジャイロスコープのゼロ バイアス誤差に対して 1 次線形近似が行われます。これは、バイアスの計算が PVQ の事前積分プロセスに含まれており、対応する事前積分を再実行する必要があるためです。計算の複雑さを軽減するために、加速度計とジャイロスコープのバイアスは IMU 誤差に対して 1 次線形化されるため、バイアスが更新されるときはパラノイド 1 次近似補償のみが必要になります。元の統合前の結果に追加されます。

解決プロセスは次のとおりです。

16. 視覚的慣性は速度、スケール、重力加速度に一致します

視覚的慣性アライメントは主に何を行いますか: 現時点では、純粋な視覚の下での各瞬間のカメラのポーズと 3D ポイントが完成しています (スケールがないため、ポーズは重力の方向と整列していません)。また、隣接するフレームIMU の事前統合が完了しました。次のステップでは、視覚的な推定結果と IMU 事前統合測定を組み合わせて、姿勢と 3D ポイントのスケールを復元し、姿勢を重力の方向に合わせて推定します。画像フレームの各フレームに対応する速度。

まず、各フレームに対応する IMU 座標系の速度、スケール、および重力加速度を定義します。

次に、変位と速度に関する 2 つの連続するフレーム k および k+1 フレームの事前積分に従って、次の式を取得できます。

上記 2 つの式によれば、次の線形制約式が得られます。

次に、最小二乗問題を作成して解決します。

最後に、対応する IMU 座標系の速度、スケール、重力加速度、および各フレームの初期値が取得されます。

17. 重力リファインメント

ほとんどの場合、重力は実際にはわかっており、テーブルを調べるなどの方法で局所的な重力加速度を知ることができます。問題を解決しようとしている実際の重力加速度が重力と異なる場合は、何か問題があるはずです。したがって、VINS の初期化では、VINS も調整する必要があります。推定された重力加速度を、重力加速度の既知の大きさ 9.81 で調整します。計算された重力の方向を考慮してサイズを調整します。

重力の大きさが既知であるため、重力加速度は元の 3 自由度から 2 自由度に変化します (元の重力は xyz の 3 方向の成分で構成され、重力係数が既知になります。) xy 方向の成分がわかっているので、z 方向の成分を解くことができます)。このとき、重力は接平面上でそれぞれの重み（x、y方向の成分）を乗算した直交する2つの単位ベクトルb1、b2に分解できます。

重力加速度は次の式で表すことができます。

次に、この表現を使用して、以前の各フレームに対応する IMU 座標系の速度、スケール、および重力加速度を解くときに対応する g_c0 を置き換え、反復最適化を通じて重力方向の単位ベクトルを収束させます。その中でb=[b1,b2]、、、W=[w1,w2]。

このとき、ワールド座標系とカメラ座標系c0との間の回転q_c0_wを求めることができる。次に、すべての変数を参照フレーム (・)^c0 からワールドフレーム (・)^w まで回転します。ボディ座標系の速度もワールド座標系に回転されます。ビジュアル SfM の変換行列は、測定単位に合わせてスケーリングされます。この時点で、初期化プロセスが完了し、これらすべてのメトリクスが密結合された単眼 VIO に入力されます。

18. 密結合単眼 VIO

スライディング ウィンドウに基づく密結合単眼 VIO は、主にスライディング ウィンドウ内の画像フレームと IMU 事前積分データに基づいて残差関数を確立し、残差関数を最小限に抑えることで最小の誤差で現在のフレームの姿勢を解決します。最適化変数のスケールを制御するために、スライディング ウィンドウは、過去の画像フレームの姿勢と観測情報を無視し、他の瞬間の姿勢推定にアプリオリ情報として参加します。

これには、周縁化ルール、周縁化事前誤差、IMU 誤差カルマン、および視覚残差の定義が含まれます。

19. VIO 最適化変数の構成

現在のスライディング ウィンドウの各フレームは、IMU の状態、カメラと IMU の外部パラメータ、および視覚観察の逆深度情報に対応します。

• PVQ と加速度計とジャイロスコープのバイアスを含む IMU 状態
• IMU回転外部パラメータは初期化中に疎結合および校正され、平行移動外部パラメータは測定によって取得され、フレームごとに最適化されます。
• 逆深さ情報は、より遠くのポイントによって生成される誤差を減らすために使用され、より遠くのポイントを表現でき、ガウス分布によりよく一致します。
  

20. 密結合VIO残差関数

周辺化によって得られた事前情報に基づいて、IMU 測定残差、視覚的測定残差、および各フレームの対応する状態は、スライディング ウィンドウ プロセス中にすべての測定残差を最小化することによって取得されます。

21. VINSにおける疎外化

1. 周縁化ルール:
これは、周縁化によって引き起こされるフィルイン問題を軽減するために行われます。周縁化された状態と他のフレームとの間の制約が少ないほど、フィルイン問題が発生する可能性は低くなります。そうでない場合は、一部の情報を破棄することをお勧めします。また、疎外されていません（OKVINS の疎外戦略）。

• 次に新しいフレームがキーフレームの場合、最も古いフレームの状態、視覚測定値、および IMU データを無視します。
• 次の新しいフレームがキーフレームではない場合、次の新しいフレームの視覚的な測定値を破棄し、フレームのポーズと IMUpose を維持します。

視覚的測定は特徴点の逆深度情報を指し、IMU データは PVQ および加速度計ジャイロスコープのバイアスです。

2. vins での周縁化によって引き起こされる線形点の不一致問題の解決 周縁化は
線形化点の早期固定につながり、次善の推定結果につながる可能性があります。ただし、著者は VIN では FEJ 戦略を使用せず、VIO では小さなドリフトが許容されるため、周縁化による悪影響は重要ではないと考えられます。

22. 視覚的な残差計算

視覚残差は特徴点の再投影誤差であり、Vins は新しい投影誤差記述を採用して、単位球上のカメラの測定残差を定義します。

ここで単位球の接平面を正規化平面とする. カメラの光学中心から正規化平面上の特徴点までのベクトルが存在する. 動き推定により, フレーム i からフレーム j に特徴点を投影し, 取得したj 番目のフレーム上の特徴点と光学中心を結んで 2 つのベクトルを取得し、単位ベクトルに正規化して接平面座標系 (正規化平面) にマッピングし、マッピングされたベクトルを計算します。視覚的な残差としての差

一般に、これは極座標制約 (ORB-SLAM ではない) の視覚的残差計算よりも 1 段階進んだものです。特徴点を正規化された平面に投影した後、座標は単位ベクトルに正規化されます。その目的は、ノイズや不一致などを減らすことです。 . 姿勢推定への影響。

23. IMU残差の意味

IMU 残差は依然としてエラー カルマン フィルター モデルに基づいており、推定された動きと事前積分によって取得された動きの間の差が計算されます。

K 回の最適化推定と k 回から k+1 回までの事前積分によって得られた imu から世界座標系への imu の姿勢、速度、回転は、次の時点で解かれることが知られています。 k+1回

残差関数における時間 k での状態 pvq とバイアスは事前に知られており、時間 k+1 での状態が推定される量です。残差の物理的意味は、推定値間の差を作ることです。 2つのフレームとimuの事前統合により、測定値の差が最小限に抑えられます

残差は主に 5 つの部分で構成されます。

• 変位の残差: 時刻 k での姿勢の推定値から時刻 k+1 での姿勢の推定値を減算して相対変位を取得し、Imu の測定値を事前積分して相対変位を取得します。残差は2 つの相対量の差
• 速度と回転
• IMU のバイアス残差は、k+1 時間の残差から k 時間の残差を差し引いた値を直接使用します (積分では、2 つのフレーム間の IMU データの加速度計とジャイロスコープの残差は一時的に変化していないと見なされます)。

24. ループ検知

DBOW ライブラリを使用して、バッグオブワード ベクトルに基づいて候補ループ キーフレームを検出します。ループ終了検出を実行するために、vins はオプティカル フロー トラッキングのコーナー ポイントに基づいて 500 個の追加特徴を抽出し、BRIEF 記述子を計算して、抽象化します。画像をバッグオブワードベクトルに変換して保存します。新しいキーフレームに対してワードバッグベクトルの類似度を計算し、ループバック候補フレームを判定します。

さらに、このアルゴリズムはマップの保存およびロード機能を提供し、ロードされたマップはループ クロージャ検出のための候補マッチング ライブラリとしても存在します。

24.1 機能の回復

まずループクロージャ関係を持つキーフレーム間で記述子ベースの特徴マッチングを実行し、RANSACに基づいて2D-2Dエッセンシャルマトリックスと3D-2D PNPを用いて外れ値をフィルタリングし、相対的な位置関係を計算する。インライアの数が要件を満たした場合、候補ループバック フレームとして識別されます。

24.2 密結合再配置

現在のスライディング ウィンドウのポーズとループバック フレームとのコモン ビュー関係に基づいて制約関係を確立するためのものであり、最適化変数は引き続き現在のスライディング ウィンドウの IMU 状態、カメラと IMU の外部パラメーター、特徴点の深さの逆数。

違いは、ループ フレームのポーズ情報が視覚残差部分にアプリオリに追加され、ループ フレームの特性と現在のスライディング ウィンドウ内のキー フレームのマッチングと制約が追加されることです。

25. センサーの遅延推定

中心となるアイデアは、遅延時間に基づいて 2 つの隣接するフレーム時間の間を補間することです。

著者はこれらの問題を解決するために特徴点を移動する方法を採用した。著者は、カメラの動きは短時間では均一な動きであると仮定しました。仮定に基づいて、カメラ平面上の特徴点の動きが計算されます。異なる時刻におけるカメラへの投影を使用すると、中央の特定の瞬間における特徴点の座標を簡単に計算できます (特徴点の速度も一定の速度で移動していると仮定します)。

特徴点が 3D 座標で表されている場合、特徴点の投影座標、特徴点の速度、および補償時間を直接使用して、対応する時刻における画像上の特徴点の座標を計算できます。

vins ではロボットの姿勢推定は IMU を中心に行われるため遅延の処理が発生し、ORB-SLAM で説明されているように画像の各フレームのタイムスタンプが厳密で対応できる保証はありません. IMU データは一貫性を保ちます。vinsにおけるこの現象の処理は、特徴点の移動速度と時間差に応じて仮想的な拘束関係を構築することである。

26. vins-fusion の改良

1. 両眼、両眼 VIO および GPS との統合を追加
2. 双方向のオプティカルフロー検査機構を導入し、特徴点を次のフレームまで追跡した後、次のフレームから前のフレームまで逆に追跡し、追跡結果に対して、同じ特徴点に対応する状態ビットを取得するが同じである必要があり（正方向追跡成功と逆方向追跡成功）、前後で追跡された特徴点のピクセル差が閾値（0.5）未満であれば、特徴点の追跡に成功したと判断されます。
3. フレーム間オプティカル フロー トラッキングのピラミッド レイヤーの数を最適化する予測メカニズムの導入
4. 両眼モードでの視覚的な初期化は比較的簡単で、カメラの視差を通じて 3D ポイントを直接取得できます。

27. オプティカル フロー ピラミッドの層数を最適化する予測メカニズムの導入

ここでは等速モデルが定義されており、等速モデルと現在のフレームの姿勢から次のフレームの姿勢を事前に予測し、現在のフレームの画像座標系の3D点を次のフレームに投影します。次のフレームの予測された姿勢に応じたフレーム イメージ プレーン、オプティカル フロー トラッキングの初期値を提供します。

予測が成功すれば初期オプティカルフローが得られるため、ピラミッドの多層化は不要となり、オプティカルフロー追跡の画像ピラミッドは4層から2層に変更される。2 レベルのピラミッドに基づいて追跡されるポイントの数が比較的少ない場合 (<10)、追跡には 4 レベルのピラミッドが再度使用されます。

28. 双方向オプティカルフローの検査仕組みを紹介

前のフレームから次のフレームまで特徴点を追跡した後、再度逆追跡を実行します。つまり、次のフレームの特徴点を使用して前のフレーム画像上で追跡します。追跡結果は、次のフレームに対応する状態ビットを満たさなければなりません。同じ特徴点 同じ (順方向追跡が成功し、逆方向追跡も成功)、順方向追跡と逆方向追跡の特徴点間のピクセル差が閾値 (0.5) 未満です。

判定条件を満たさない特徴点については、対応するステータスビットを0にリセットします。

29. 両眼モードでのオプティカル フロー トラッキング

両眼カメラの場合、左目から右目へのオプティカルフロートラッキングのステップが実行され、左目と右目の特徴点のデータ関連付けが実現されます。同様に、双方向のオプティカル フロー トラッキングが左右のオブジェクト間で実行されます。

左目と右目の間のオプティカル フロー トラッキングでは、オプティカル フロー トラッキングに 4 層のピラミッドが使用されます。

30. バックエンド最適化における両眼再投影残差

拘束される i 番目のフレームと j 番目のフレームが同じフレームではない場合: i 番目のフレームの左目と j 番目のフレームの左目の間の視覚残差は vins-mono と同じです。そして、i 番目のフレームの左目と j 番目のフレームの右目が再投影されます。投影された視覚残差の中央には、外部パラメータ変換の追加のステップがあります。右のフレームを変換する必要があります。 -目の特徴は、再投影誤差を計算する前に左目のカメラ座標系を指します。

拘束対象となるi番目のフレームとj番目のフレームが同じフレームの場合：姿勢に基づいて拘束関係を確立することはできないが、外部パラメータと特徴点の奥行き情報に基づいて拘束関係を確立することができる解決策のために。

4. 外れ値の選別

1. バックエンドに外れ値のスクリーニング機構を追加して、
スライディング ウィンドウ内の画像フレームを横断し、特定の特徴点について、点が観察される最初のフレームと他のフレームとの間の再投影誤差を、また両眼モードで計算します。参照フレームの左目と現在のフレームの右目の間の再投影誤差を計算し、最終的に平均再投影誤差を求める場合、再投影誤差が 3 ピクセルより大きい場合、そのポイントは外れ値とみなされます。

5. GPS フュージョン

GPS は主に、スラム システムの絶対姿勢推定情報を提供します。

global_fusion ノードでは、主に vio のポーズと GPS 信号をサブスクライブし、vio のポーズを GPS 座標系に変換してユニットを統合します。SLAM システムの実行後、最初のフレームに対応する GPS 緯度と経度が SLAM システムの開始点および基準フレームとして使用され、後続の姿勢はこの基準フレームに基づいて更新されます。

制約関係を構築する際、コスト関数は主に VIO 間のフレーム間姿勢制約、GPS の絶対位置 (x、y、z)、および VIO 姿勢と GPS 位置の間の回転変換関係を使用して設計されます。