マイルメータ航跡推定とIMU予備積分

参考:VINSコード注釈版
プリポイント、理解しにくい点は主に、ヤコビの計算、どのように反復的に共分散を得るのか、deltaBiasを使ってプリポイントを更新するのは何ですか?実は、これは2フレームの画像の間の輪式距離計の共分散を計算する過程と似ていて、ただ距離計は比較的に簡単で、 を使って、更に共方差伝達の法則を結びつけて、得ることができて、だから簡単に理解することができて、距離計の中の航跡の推論はIMUの予積分の中の PVQ に対応します(速度と変位の予備積分は加速度測定値の重力gを除去していないことに注意),すなわち,予積分が計算されるもの,予積分がはっきりすると測定値であり,測定値にはnoiseがあり,航跡推論のnoise我々はどのように計算するかを知っているが,IMU予積分のnoiseも同様であるが,少し面倒なだけで,誤差運動学方程式を用いて,最後に以下の状態方程式を得ることができる.˙ = A x + B w , w ∼ N ( 0 , Σ )\dot x = Ax+Bw, w\sim N(0,\Sigma) x˙=Ax+Bw,w∼N(0,Σ)
A A AとB B Bは常行列ではないので、これは線形時変連続系であり、制御理論を学んだことがあるこの式はよく知られているだろう.サンプリング周期が小さいので、近似離散化が可能であり、さらにx(k+1)=(I+δ t A ) x ( k ) + δ t B w ( k ) , w ( k ) ∼ N ( 0 , Σ k ) x (k+1)= (I +\delta tA)x(k)+\delta t Bw(k), w(k)\sim N(0,\Sigma_k) x(k+1)=(I+δtA)x(k)+δtBw(k),w(k)∼N(0,Σk​)
注意、ここのw(k)w(k)w(k)w(k)すでに離散的ノイズになっていますが、IMUの連続ノイズをどのように離散ノイズに変換するかについてはKalibrを使ったことがある方がおなじみでしょう.i枚目のピクチャからi+1枚目のピクチャまでの間のIMUの予備積分に対する共分散を得るために、共分散の伝播律はよく知られていますが、その予備積分の共分散は上式に対して反復計算を行います.それ以外に、IMUに対して予備積分時になぜヤコビ行列を計算するのか,後に初期値でテイラー展開を行うためである(実際にはbiasに対するヤコビのみを用いた).IMUデータが来るごとに、ヤコビ行列と共分散行列を一度押して、最後に最後のフレームのIMUデータを処理した後、事前積分が完成して、共分散があって、それから順理に章を作って最適化を始めました.上のこの実現はIntegrationBase::midPointIntegration()関数の中で
プリインテグレーションの結果は実は測定値で、私たちのカメラで観測した特徴点の座標、マイルメーターの測定値など、唯一の違いは、プリインテグレーションが測定値として本来変わらないことですが、この測定値には最適化変数biasが含まれているため、プリインテグレーション式のプッシュ時にIMUを世界座標系に相対する(w)の姿勢、速度が前のフレーム画像に対して相対的に変換されるとIMUの座標系が結ばれる(すなわち、b_k)が、biasが何も処理していないのでbiasは残っている(実はbias自体の変化は小さく、2フレーム画像の間にはわずかな変化しかなく、IMUの姿勢とは異なり、時刻ごとに大きく異なる)ああ、この予備積分に最適化変数biasが含まれている以上、biasが変わったら、私たちは自然に予備積分を再計算しなければなりません.どうすればいいのでしょうか.直接的な方法は、新しいbiasを使って予備積分を再計算することです.この方法はもちろん可能ですが、最適化変数を分離する理由とは違います.(最適化変数の更新後にプリインテグレーションを再計算しないためにね)、どうすればいいのか、このbiasの変化が小さいという常識を覚えていますか、自然とテイラー展開(近似)を使いたいと思っています.テイラー展開にはヤコビが必要ですね(つまり、プリインテグレーション対k時点でbiasの勾配の転置)ああ、このヤコビは私たちが予備積分をしたばかりの時は過ぎたでしょう.だから、予備積分の最後の神秘的なベールは^~^の上に開いています.この実現はIntegrationBase::evaluate()関数内にあります.
ちょうど最適化が始まるという話ですが、IMUプリインテグレーションの結果が測定値です.私たちはどのように測定値を使って最適化するのでしょうか.カメラを使って地図点の観測構造の残差をどのようにしたか覚えていますか.ここでも同じですが、実はプリインテグレーションを導いたときに残差が得られました(式5)あ、ただ私たちは残差の形式に書かれていないだけで、項目を移す必要があり、結果が一目瞭然です(式24)次に, ,例えばGauss Newton法で増分を与え,テイラー展開により分離し,この増分の線形方程式を得た後,線形最小二乗を構成し,この線形最小二乗を解くことによって目標関数を最小にし,つまりこの増分がいくらであるべきかを求め,この増分を前回の最適化変数の値に加算することで得た.今回の最適化変数の値は,ずっとループしていくと最終的な最適化変数の結果が得られるが,テイラー展開を行う際にはヤコビを用いる必要があり,計算ヤコビは最適化される変数の初期値を用いる(収束のためには初期値を用いる必要がある)あ、1回目の最適化の時だけヤコビを計算するのに初値を使い、その後使用した前回の最適化結果を線形化点としてヤコビを計算します.ヤコビ行列を繰り返し更新する必要があるので、時間がかかるのは避けられませんが、何か良い方法はありませんか?SVOは加速ですが、inverse compositional formulation方式でヤコビ行列の上のこの実を計算するだけです現在IMUFactor::Evaluate()関数内
@leatherwang