参考リンク：OpenVINS https://docs.openvins.com/index.html
4.測定更新の導出
4.1 最小平均二乗誤差 (MMSE) 推定
次の静的状態推定問題を考えてみましょう: nnが与えられた場合n次元ガウスランダムベクトルx ∽ N ( x ^ ㊀ , P xx ㊀ ) x\backsim \mathcal{N}(\hat x^{㊀},P^{㊀}_{xx})バツ∽N (バツ^㊀、Pxx㊀)、および新しいmmm次元の状態に依存しないゼロ平均白色ガウス ノイズn ∽ N ( 0 , R ) n\backsim \mathcal{N}(0,R)n∽N ( 0 ,R) 的观测 z m = z + n = h ( x ) + n z_m = z+n=h(x)+n zメートル=z+n=h ( x )+n 、事後 PDF の最初の 2 つの (中心) モーメントp ( x ∣ zm ) p(x|z_m) を計算したいとします。p ( x ∣ zメートル）。一般に（非線形測定モデルが与えられた場合）、事後確率密度関数を次のように近似します。p ( x ∣ zm ) ≈ N ( x ^ ⊕ , P xx ⊕ ) p(x|z_m) \estimate \mathcal{N}(\ hat x^{\oplus},P^{\oplus}_{xx})p ( x ∣ zメートル）≈N (バツ^⊕、Pxx⊕）。設計上、これはKay 1993の MMSE 推定問題に対する (近似的な) 解決策です。
4.2 条件付き確率分布
このために、ベイズ規則を使用します:
p ( x ∣ zm ) = p ( x , zm ) p ( zm ) p(x|z_m)={p(x,z_m) \over p(z_m ) }p ( x ∣ zメートル）=p ( zメートル）p ( x ,zメートル)
一般に、この条件付き確率密度関数は、単純化する仮定を課すことなく分析的に計算することはできません。現在の問題では、まずp ( zm ) ≈ N ( z ^ , P zz ) p(z_m)\estimate \mathcal{N}(\hat{z},P_{zz}) を近似します。p ( zメートル）≈N (z^、Pズズ) (これが実際に当てはまる場合)、次の結合ガウス pdf が得られます (結合ガウス pdf は依然としてガウスであることに注意してください):
p ( x , zm ) = N ( [ x ^ ㊀ z ] , [ P xx P xz P zx P zz ] ) = : N ( y ^ , P yy ) p(x,z_m)=\mathcal{N}\begin{pmatrix} \left[ \begin{array}{ccc} \hat x^{ ㊀}\\ z \\ \end{array} \right],\left[ \begin{array}{ccc} P_{xx} & P_{xz} \\ P_{zx} & P_{zz}\\ \ end{array} \right] \\ \end{pmatrix}=: \mathcal{N}(\hat y, P_{yy})p ( x ,zメートル）=N([バツ^㊀z]、[PxxPzx _Pxz _Pズズ]）=:N (y^、Pやあ)
これら 2 つのガウス分布を最初の方程式に代入すると、次の条件付きガウス確率密度関数が得られます。
p ( x ∣ zm ) ≈ N ( y ^ , P yy ) N ( z ^ , P zz ) p(x|z_m) \estimate{ \mathcal{N}(\hat y,P_{yy} ) \over \ mathcal{N}(\hat z,P_{zz} )}p ( x ∣ zメートル）≈N (z^、Pズズ）N (y^、Pやあ)= 1 ( 2 π ) n ∣ P yy ∣ / ∣ P zz ∣ e − 1 2 [ ( y − y ^ ) ⊤ P yy − 1 ( y − y ^ ) − ( zm − z ^ ) ⊤ P zz − 1 ( zm − z ^ ) ] ={1 \over \sqrt{(2 \pi)^n|P_{yy}|/|P_{zz}|}}e^{-{1 \over 2}[(y - \hat y)^{\top}P_{yy}^{-1}(y-\hat y)-(z_m-\hat z)^\top P^{-1}_{zz}(z_m- \hatz)]}=( 2P ) _n ∣Pやあ∣/ ∣Pズズ∣1e−21[( y −y^）⊤P _やあ− 1( y −y^) − ( zメートル−z^ )⊤P _グーグー− 1( zメートル−z^ )]= : N ( x ^ ⊕ , P xx ⊕ ) =:\mathcal{N}(\hat x^{\oplus},P^{\oplus}_{xx})=:N (バツ^⊕、Pxx⊕)
ここで、条件付き平均を導出し、共分散は次のように計算できます。 まず、分母項∣ P yy ∣ / ∣ P zz ∣ |P_{yy}|/|P_{zz}| を∣P _やあ∣/ ∣Pズズ∣ P yy ∣ = ∣ [ P xx P xz P zx P zz ] ∣ = ∣ P xx − P xz P zz − 1 P zx ∣ ∣ P zz ∣ |P_{yy} |= \ .
∣P _やあ∣=
[PxxPzx _Pxz _Pズズ]
=
Pxx−Pxz _Pグーグー− 1Pzx _
Pズズ
P zz P_{zz} であると仮定します。Pズズこれは可逆的であり、シュール補体の決定的特性を使用します。したがって、次のようになります。
∣ P yy ∣ ∣ P zz ∣ = ∣ P xx − P xz P zz − 1 P zx ∣ ∣ P zz ∣ = ∣ P xx − P xz P zz − 1 P zx ∣ {|P_{yy }| \over |P_{zz}|} = {\begin{vmatrix} P_{xx} -P_{xz}P^{-1}_{zz}P_{zx} \\ \end{vmatrix} \over |P_{zz}|} = \begin{vmatrix} P_{xx} -P_{xz}P^{-1}_{zz}P_{zx} \\ \end{vmatrix}∣P _ズズ∣∣P _やあ∣=∣P _ズズ∣
Pxx−Pxz _Pグーグー− 1Pzx _
=
Pxx−Pxz _Pグーグー− 1Pzx _
次にエラー状態を定義してrx = x − x ^ ㊀ r_x = x - \hat x^{㊀}r×=バツ−バツ^㊀，rz = zm − z ^ r_z = z_m -\hat zrz=zメートル−z^，ry = y − y ^ r_y = y - \hat yrはい=y−y^そして、行列の内部補題を使用して、指数項を次のように書き換えます:
( y − y ^ ) ⊤ P yy − 1 ( y − y ^ ) − ( z , − z ^ ) ⊤ P zz − 1 ( zm − z ^ ) (y - \hat y)^{\top}P_{yy}^{-1}(y-\hat y)-(z_,-\hat z)^\top P^{-1}_{zz } (z_m-\hat z)( y−y^）⊤P _やあ− 1( y−y^）−( z、−z^ )⊤P _グーグー− 1( zメートル−z^ )= ry ⊤ P yy − 1 ry − rz ⊤ P zz − 1 rz =r_y^{\top}P_{yy}^{-1}r_y-r_z^\top P^{-1}_{zz} r_z=ry⊤Pやあ− 1rはい−rz⊤Pグーグー− 1rz= [ rxrz ] ⊤ [ P xx P xz P zx P zz ] − 1 [ rxrz ] − rz ⊤ P zz − 1 rz =\left[ \begin{array}{ccc} r_{x} \\ r{z} \\ \end{配列} \right]^\top \left[ \begin{配列}{ccc} P_{xx} & P_{xz} \\ P_{zx} & P_{zz}\\ \end{配列} \right] ^{-1} \left[ \begin{array}{ccc} r_{x} \\ r{z}\\ \end{array} \right] - r_z^\top P^{-1 }_{zz}r_z=[r×rz _]⊤[PxxPzx _Pxz _Pズズ]− 1[r×rz _]−rz⊤Pグーグー− 1rz= ( rx − P xz P zz − 1 rz ) ⊤ ( P xx − P xz P zz − 1 P zx ) − 1 ( rx − P xz P zz − 1 rz ) =(r_x - P_{xz}P_{zz }^{-1}r_z)^\top(P_{xx} - P_{xz}P^{-1}_{zz}P_{zx})^{-1}(r_x - P_{xz}P^ {-1}_{zz}r_z)=( r×−Pxz _Pグーグー− 1rz）⊤ (Pxx−Pxz _Pグーグー− 1Pzx _）− 1 (r×−Pxz _Pグーグー− 1rz)
ここで、( pzz − 1 ) ⊤ = P zz − 1 (p_{zz}^{-1} )^\top = P_{zz}^{-1}( pグーグー− 1）⊤=Pグーグー− 1共分散行列は対称なので。これまでのところ、次のように条件付きガウス確率密度関数を構築できます。
p ( xk ∣ zm ) = 1 ( 2 π ) n ∣ P xx − P xz P zz − 1 P zx ∣ × exp ( − 1 2 [ ( rx − P xz P zz − 1 rz ) ⊤ ( P xx − P xz P zz − 1 P zx ) − 1 ( rx − P xz P zz − 1 rz ) ] ) p(x_k|z_m)= {1 \over \sqrt {(2\pi)^n|P_{xx}-P_{xz}P_{zz}^{-1}P_{zx}|}}\times exp \begin{pmatrix} -{1\over 2 } [ (r_x - P_{xz}P_{zz}^{-1}r_z)^\top(P_{xx} - P_{xz}P^{-1}_{zz}P_{zx})^{ -1 }(r_x - P_{xz}P^{-1}_{zz}r_z)]\\ \end{pmatrix}p ( xｋ∣z _メートル）=( 2P ) _n ∣Pxx−Pxz _Pグーグー− 1Pzx _∣1×e × p(−21[( r×−Pxz _Pグーグー− 1rz）⊤ (Pxx−Pxz _Pグーグー− 1Pzx _）− 1 (r×−Pxz _Pグーグー− 1rz) ]）
これにより、求めている次の条件付き平均と共分散が得られます。
x ^ ⊕ = x ^ ㊀ + P xz P zz − 1 ( zm − z ^ ) \hat x^{\oplus} = \hat x^{㊀} + P_{xz}P_{zz}^{-1}(z_{m}-\hat z)バツ^⊕=バツ^㊀+Pxz _Pグーグー− 1( zメートル−z^ )P xx ⊕ = P xx ㊀ − P xz P zz − 1 P zx P_{xx}^{\oplus} = P_{xx}^{㊀}-P_{xz}P_{zz}^{-1} P_{zx}Pxx⊕=Pxx㊀−Pxz _Pグーグー− 1Pzx _
これらは、(線形) 状態推定の基本方程式です。
4.3 線形測定の更新
特殊なケースとして、線形 MMSE 推定量を説明するための単純な線形測定モデルを検討します。
zm , k = H kxk + nk z_{m,k} = H_kx_k + n_kzm 、 k=Hｋバツｋ+nｋz ^ k : = E [ zm , k ] = E [ H kxk + nk ] = H kx ^ k ㊀ \hat z_k:=\mathbb{E}[z_{m,k}] = \mathbb{E}[ H_kx_k+n_k] = H_k\hat x_k^{㊀}z^ｋ:=E [ zm 、 k]=E [ Hｋバツｋ+nｋ]=Hｋバツ^k㊀
これにより、次のように共分散行列と相関行列を導出できます。
Pzz = E [ ( zm , k − z ^ k ) ( zm , k − z ^ k ) ⊤ ] P_{zz} = \mathbb{E} [( z_{m,k}-\hat z_k)(z_{m,k}-\hat z_k)^{\top}]Pズズ=E [( zm 、 k−z^ｋ) ( zm 、 k−z^ｋ）⊤ ]= E [ ( H kxk + nk − H kx ^ k ㊀ ) ( H kxk + nk − H kx ^ k ㊀ ) ⊤ ] = \mathbb{E}[(H_kx_k+n_k-H_k\hat x_k^{㊀ })(H_kx_k + n_k-H_k \hat x_k^{㊀})^\top]=E [( Hｋバツｋ+nｋ−Hｋバツ^k㊀) ( Hｋバツｋ+nｋ−Hｋバツ^k㊀）⊤ ]= H k P xx ㊀ H k ⊤ + R k =H_kP_{xx}^{㊀}H_{k}^\top+R_k=HｋPxx㊀Hk⊤+Rｋ
ここでR k R_kRｋは離散測定ノイズ行列H k H_kHｋは状態を測定領域にマッピングする測定ヤコビアン行列です、P xx ㊀ P_{xx}^{㊀}Pxx㊀は現在の状態の共分散です。
P xz = E [ ( xk − x ^ k ㊀ ) ( zm , k − z ^ k ) ⊤ ] P_{xz} = \mathbb{E}[(x_{k}-\hat x^{㊀}_k) (z_{m,k}-\hat z_k)^{\top}]Pxz _=そして[( xｋ−バツ^k㊀) ( zm 、 k−z^ｋ）⊤ ]= E [ ( xk − x ^ k ㊀ ) ( H kxk + nk − H kx ^ k ㊀ ) ⊤ ] = \mathbb{E}[(x_{k}-\hat x^{㊀}_k)( H_kx_k + n_k-H_k \hat x_k^{㊀})^\top]=そして[( xｋ−バツ^k㊀) ( Hｋバツｋ+nｋ−Hｋバツ^k㊀）⊤ ]= P xx ㊀ H k ⊤ =P_{xx}^{㊀}H_{k}^\top=Pxx㊀Hk⊤
私たちは、ノイズが状態に依存しないという事実を利用します。これらの量を基本方程式に代入すると、次の更新された方程式が得られます。
x ^ k ⊕ = x ^ k ㊀ + P k ㊀ H k ⊤ ( H k P k ㊀ H k ⊤ + R k ) − 1 ( zm , k − z ^ k ) \hat x_k^{\oplus} = \hat x^{㊀}_{k} + P_{k}^{㊀}H_k^\top(H_kP_k^{㊀}H_k^\top+R_k)^ { -1}(z_{m,k}-\hat z_k)バツ^k⊕=バツ^k㊀+Pk㊀Hk⊤( HｋPk㊀Hk⊤+Rｋ）− 1 (zm 、 k−z^ｋ) = x ^ k ㊀ + K rz =\hat x_k^{㊀} +Kr_z=バツ^k㊀+Kr _z
P xx ⊕ = P k ㊀ + P k ㊀ H k ⊤ ( H k P k ㊀ H k ⊤ + R k ) − 1 ( PK ㊀ H k ⊤ ) ⊤ P_{xx}^{\oplus} = P^{ ㊀}_{k} + P_{k}^{㊀}H_k^\top(H_kP_k^{㊀}H_k^\top+R_k)^{-1}(P_K^{㊀}H^\top_k)^\上Pxx⊕=Pk㊀+Pk㊀Hk⊤( HｋPk㊀Hk⊤+Rｋ）− 1 (PK㊀Hk⊤）⊤ = P k ㊀ + P k ㊀ H k ⊤ ( H k P k ㊀ H k ⊤ + R k ) − 1 ( H k PK ㊀ ) = P^{㊀}_{k} + P_{k}^{ ㊀}H_k^\top(H_kP_k^{㊀}H_k^\top+R_k)^{-1}(H_kP_K^{㊀})=Pk㊀+Pk㊀Hk⊤( HｋPk㊀Hk⊤+Rｋ）− 1 (HｋPK㊀）
これらは本質的にカルマン フィルター (または線形 MMSE) 更新方程式です。
5. 特徴の三角形分割
5.1 3D デカルト三角測量
私たちは、フィーチャの 3D デカルト位置を最初に推定できる解決可能な線形システムを作成したいと考えています。これを行うために、特徴が既知の量として見られるすべてのポーズを扱います。これらの特徴は、任意に選択したカメラ座標系{ A } \{A\}に含めることができます。{ A }の下で三角形を作成します特徴点pf p_fpふ指定された座標系{ A } \{A\}で指定できます{ A }の下のポーズは1...m 1...m です1... mが観察されました。いくつかのカメラのポーズからC i , i = 1... m C_i,i=1...mC私は、私=次の変換は1... mの下で得られます。
C ipf = AC i R ( A pf − A p C i ) ^{C_i}p_f = ^{C_i}_AR(^Ap_f-^Ap_{C_i})C私はpふ=あC私はR (Ａｐ _ふ−あpC私は) A pf = AC i R ⊤ C ipf + A p C i ^A{p_f}=^{C_i}_AR^{\top}{^{C_i}p_f}+^Ap_{C_i}Ａｐ _ふ=あC私はR⊤C私はpふ+あpC私は
ノイズがない場合、現在の座標系での測定値は方向C ib ^{C_i}bになります。C私はbとその深さC iz ^{C_i}zC私はしたがって、現在のフレームから見られる特徴の次のマッピングがあります:
C ipf = C izf C ibf = C izf [ unvn 1 ] ^{C_i}p_f=^{C_i}z_f^{C_i}b_f = ^{ C_i}z_f\left[ \begin{array}{ccc} u_{n} \\ v_{n}\\ 1\\ \end{array} \right]C私はpふ=C私はzfC私はbふ=C私はzふ
あなたんvん1
ご注意くださいあなたん 和 v n v_n vん歪みのない正規化された画像座標を表します。この方向は、上記の式を代入することで固定座標系に変換できます:
A pf = AC i R ⊤ C ipf + A p C i ^A{p_f}=^{C_i}_AR^{\top}{^{C_i }p_f}+^Ap_{C_i}Ａｐ _ふ=あC私はR⊤C私はpふ+あpC私は= C izf A b C i → f + A p C i =^{C_i}z_f{^Ab_{C_i\rightarrow f}}+^Ap_{C_i}=C私はzふAB _C私は→ふ+あpC私は
推定深度C izf ^{C_i}z_f を消去するにはC私はzふ追加の自由度のニーズを満たすために、方位角に直交する次のベクトルA b C i → f ^Ab_{C_i \rightarrow f}を定義します。AB _C私は→ふ：
AN i = ⌊ A b C i → f × ⌋ = [ 0 − A b C i → f ( 3 ) A b C i → f ( 2 ) A b C i → f ( 3 ) 0 − A b C i → f ( 1 ) − A b C i → f ( 2 ) A b C i → f ( 1 ) 0 ] ^AN_i=\lfloor {^Ab_{C_i \rightarrow f} \times} \rfloor=\left[ \ begin{array}{ccc} 0 & -^Ab_{C_i \rightarrow f} (3)& ^Ab_{C_i \rightarrow f}(2)\\ ^Ab_{C_i \rightarrow f} (3) & 0 & - ^Ab_{C_i \rightarrow f} (1)\\ -^Ab_{C_i \rightarrow f} (2) & ^Ab_{C_i \rightarrow f} (1) & 0\\ \end{array} \right]AN _私は=⌊AB _C私は→ふ× ⌋=
0AB _C私は→ふ( 3 )−AB _C私は→ふ( 2 )−AB _C私は→ふ( 3 )0AB _C私は→ふ( 1 )AB _C私は→ふ( 2 )−AB _C私は→ふ( 1 )0
3 本の直線はすべてベクトルA b C i → f ^Ab_{C_i \rightarrow f}に対して垂直です。AB _C私は→ふ，したがってAN i A b C i → f = 0 3 ^AN_i ^Ab_{C_i \rightarrow f}=0_3AN _私あbC私は→ふ=03。次に、変換方程式/制約を乗算して、未知の方程式にのみ関連する2 つの 3 自由度方程式A pf ^A{p_f}を形成できます。Ａｐ _ふAN i A pf = AN i C izf A b C i → f + AN i A p C i ^AN_i^A{p_f}= ^AN_i ^{C_i}{z_f} ^Ab_{C_i \rightarrow f} + ^
AN_i ^Ap_{C_i}AN _私あpふ=あN私C私はzふAB _C私は→ふ+あN私あpC私は= AN i A p C i =^AN_i^Ap_{C_i}=あN私あpC私は
すべての測定値を積み重ねることにより、次のことが可能になります:
[ ⋮ AN i ⋮ ] ⏟ AA pf = [ ⋮ AN i A p C i ⋮ ] ⏟ b \underbrace{\left[ \begin{array}{ccc} \vdots \\ ^ AN_i\ \ \vdots\\ \end{array} \right]}_{A}^A{p_f}=\underbrace{\left[ \begin{array}{ccc} \vdots \\ ^AN_i^Ap_{C_i }\ \ \vdots\\ \end{配列} \right]}_{b}あ
⋮AN _私は⋮
pふ=b
⋮AN _私あpC私は⋮
各ピクセル測定には 2 つの制約があるため、 m > 1 m>1 である限り、メートル>1では、フィーチャを三角形分割するのに十分な制約があります。実際には、特徴点のビューが多ければ多いほど、三角形分割がより適切になるため、通常は少なくとも 5 つのビューから特徴点を確認する必要があります。すべてのAN i ^AN_iを選択できますAN _私は行数を減らすために 2 行のシステムを使用しますが、正方形システムを使用すると、次の「トリック」を実行できます。
A ⊤ AA pf = A ⊤ b A^\top A ^Ap_f=A^\top bあ⊤A _Ａｐ _ふ=あ⊤ b( ∑ i AN i ⊤ AN i ) A pf = ( ∑ i AN i ⊤ AN i A p C i ) (\sum_i{^AN_i^\top {^AN_i}})^Ap_f=(\sum_i{
{ ^AN^\top _i{^AN_i}{^Ap_{C_i}}}})(私∑AN _私⊤AN _私は）Ａｐ _ふ=(私∑AN _私⊤AN _私はＡｐ _C私は）
これは 3x3 システムであり、元の 3mx3m や 2mx2m システムに比べて素早く解くことができます。また、三角測量機能が「有効」であること、カメラの正面にあり、遠すぎないことも確認します。上記の線形システムと負のシステムの条件数は、誤差と最大値データに「敏感」です。
5.2 1D 深度三角測量
私たちは、フィーチャの 1D 深度位置を最初に推定できる解決可能な線形システムを作成したいと考えています。これを行うために、フィーチャが見られるすべてのポーズを、アンカー フレーム内の方向とともに既知の量として扱います。この関数は、任意に選択できるアンカー カメラ フレーム{ A } \{A\}にカメラ フレームをアンカーします。三角測量は{
A }で実行されますこれをアンカー フレーム内の正規化された画像座標[ unvn 1 ] ⊤ \left[ \begin{array}{ccc} u_{n} &v_{n}&1\\ \end{array} \right]^ \top として定義します。[あなたんvん1]⊤。アンカー ポイント ポーズAAA、機能pf p_fpふ1...m 1...mの位置に配置可能1... mを観測すると、任意のカメラ位置 C i , i = 1... m から観測できるC_i,i=1...mC私は、私=1... m は次の変換を取得します:
C ipf = AC i R ( A pf − A p C i ) ^{C_i}p_f=^{C_i}_AR(^Ap_f -^Ap_{C_i})C私はpふ=あC私はR (Ａｐ _ふ−あpC私は) A ipf = AC i R ⊤ C ipf + A p C i ^{A_i}p_f=^{C_i}_AR^\top {^{C_i}}p_f +^Ap_{C_i}あ私はpふ=あC私はR⊤C私はpふ+あpC私はA zf A pf = AC i R ⊤ C ipf + A p C i ^Az_f^{A}p_f=^{C_i}_AR^\top {^{C_i}}p_f +^Ap_{C_i}ザ・ズfあpふ=あC私はR⊤C私はpふ+あpC私は
ノイズがない場合、現在の座標系での測定値はC ib ^{C_i}b方向です。C私はbとその深さC iz ^{C_i}zC私はz
C ipf = C izf C ibf = C izf [ unvn 1 ] ^{C_i}p_f=^{C_i}z_f^{C_i}b_f = ^{C_i}z_f\left[ \begin{array}{ccc} u_{ n} \\ v_{n}\\ 1\\ \end{配列} \right]C私はpふ=C私はzfC私はbふ=C私はzふ
あなたんvん1
ご注意くださいあなたん 和 v n v_n vん歪みのない正規化された画像座標を表します。この方向は、上記の式を代入することで固定座標系に変換できます。
A zf A bf = AC i R ⊤ C izf C ibf + A p C i ^A{z_f}^Ab_f=^{C_i}_AR^{\トップ}{^{C_i}z_f}{^{C_i}b_f} +^Ap_{C_i}ザ・ズふAB _ふ=あC私はR⊤C私はzふC私はbふ+あpC私は= C izf A b C i → f + A p C i =^{C_i}z_f{^Ab_{C_i\rightarrow f}}+^Ap_{C_i}=C私はzふAB _C私は→ふ+あpC私は
推定深度C izf ^{C_i}z_f を消去するにはC私はzふ追加の自由度のニーズを満たすために、方位角に直交する次のベクトルA b C i → f ^Ab_{C_i \rightarrow f}を定義します。AB _C私は→ふ：
AN i = ⌊ A b C i → f × ⌋ ^AN_i=\lfloor {^Ab_{C_i \rightarrow f} \times} \rfloorAN _私は=⌊AB _C私は→ふ× ⌋3
本の直線はすべてベクトルA b C i → f ^Ab_{C_i \rightarrow f}AB _C私は→ふ，したがってAN i A b C i → f = 0 3 ^AN_i ^Ab_{C_i \rightarrow f}=0_3AN _私あbC私は→ふ=03。次に、変換方程式/制約を乗算して、未知の方程式にのみ関連する2 つの 3 自由度方程式A pf ^A{p_f}を形成できます。Ａｐ _ふ( AN i A pf ) A zf = AN i C izf A b C i → f + AN i A p C i (^AN_i^A{p_f }
)^Az_f= ^AN_i ^{C_i}{z_f} ^Ab_ {C_i \rightarrow f} + ^AN_i ^Ap_{C_i}(AN _私あpふ）ザ・ズふ=あN私C私はzふAB _C私は→ふ+あN私あpC私は= AN i A p C i =^AN_i^Ap_{C_i}=あN私あpC私は
然后我们可以制定以下系统：
( ∑ i ( A N i A b f ) ⊤ ( A N i A b f ) ) A z f = ( ∑ i ( A N A b f ) i ⊤ A N i A b i ) (\sum_i{(^AN_i^Ab_f)^\top ({^AN_i}}^Ab_f))^Az_f=(\sum_i{
{(^AN^Ab_f)^\top _i{^AN_i}{^Ab_i}}}) (私∑(AN _私あbふ）⊤ (AN _私はAB _ふ) )ザ・ズふ=(私∑(AN _AB _ふ）私⊤AN _私はAB _私は）
これは 1x1 システムであり、1 回のスカラー除算ですぐに解くことができます。また、三角測量機能が「有効」であること、カメラの正面にあり、遠すぎないことも確認します。完全な特徴は次のように再構成できます。
A pf = A zf A bf ^Ap_f=^Az_f^Ab_fＡｐ _ふ=あzfあbふ
5.3 3D 逆非線形最適化
三角形分割されたフィーチャの 3D 位置を取得した後、この推定を改善するために非線形最小二乗法が実行されます。良好な数値安定性を得るために、ポイント フィーチャに対して逆深度表現を使用し、収束を支援します。ほとんどの場合、この問題は屋内環境では 2 ～ 3 回の反復以内に収束することがわかりました。特徴変換は次のように記述できます。
C ipf = AC i R ( A pf − A p C i ) ^{C_i}p_f=^{C_i}_AR(^Ap_f -^Ap_{C_i})C私はpふ=あC私はR (Ａｐ _ふ−あpC私は) = A zf AC i R ( [ A xf / A zf A yf / A zf 1 ] − 1 A zf A p C i ) =^Az_f {^{C_i}_AR}\begin{pmatrix} \left[ \begin {array}{ccc} ^Ax_f/^Az_f \\^Ay_f/^Az_f \\ 1\\ \end{array} \right] -{1\over {^Az_f}}^Ap_{C_i}\\ \end {pマトリックス}=あzふあC私はR
あ×ふ/ザ・ズふああ_ふ/ザ・ズふ1
−ザ・ズふ1Ａｐ _C私は
我们定义 u A = A x f / A z f u_A= ^Ax_f/^Az_f あなたあ=あバツふ/ザ・ズふ，v A = A yf / A zf v_A= ^Ay_f/^Az_fvあ=あyふ/ザ・ズふ，ρ A = 1 / A zf \rho_A=1/^Az_frあ=1 /ザ・ズふ以下の計算公式：
h ( u A , v A , ρ A ) = AC i R ( [ u A v A 1 ] − ρ AA p C i ) h(u_A,v_A,\rho_A) = ^{C_i}_AR \begin{pmatrix} \left[ \begin{array}{ccc} u_A\\ v_A \\ 1\\ \end{array} \right] -\rho_A{^Ap_{C_i}}\\ \end{pmatrix}h (あなたあ、vあ、rあ）=あC私はR
あなたあvあ1
−rあＡｐ _C私は
カメラ座標系から見た特徴量は次のように再定式化できます:
z = [ uivi ] z=\left[ \begin{array}{ccc} u_i\\ v_i \\ \end{array} \right]z=[あなた私はv私は] = [ h ( u A , v A , ρ A ) ( 1 ) / h ( u A , v A , ρ A ) ( 3 ) h ( u A , v A , ρ A ) ( 2 ) / h ( u A , v A , ρ A ) ( 3 ) ] =\left[ \begin{array}{ccc} h(u_A,v_A,\rho_A)(1) / h(u_A,v_A,\rho_A)(3)\ \h(u_A,v_A,\rho_A)(2) / h(u_A,v_A,\rho_A)(3)\\\end{配列}\right]=[h (あなたあ、vあ、rあ) ( 1 ) / h ( uあ、vあ、rあ) ( 3 )h (あなたあ、vあ、rあ) ( 2 ) / h ( uあ、vあ、rあ) ( 3 )] = h ( u A , v A , ρ A ) = h( u_A , v_A , \ rho_ A )=h (あなたあ、vあ、rあ）
したがって、最小二乗法とヤコビアンを次のように定式化できます。
argminu A , v A , ρ A ∣ ∣ z − h ( u A , v A , ρ A ) ∣ ∣ 2 \underset{u_A,v_A,\rho_A}{引数 }||zh(u_A,v_A,\rho_A)||^2あなたあ、vあ、rあ最小値_ _∣∣z _−h (あなたあ、vあ、rあ) ∣ ∣2 ∂ h ( u A , v A , ρ A ) h ( u A , v A , ρ A ) = AC i R [ [ 1 0 0 1 0 0 ] − A p C i ] {\partial h(u_A, v_A,\rho_A) \over h(u_A,v_A,\rho_A)} = ^{C_i}_AR\begin{bmatrix} \left[ \begin{array}{ccc} 1 & 0\\ 0&1 \\ 0&0\\ \end{配列} \right] -{^Ap_{C_i}}\\ \end{bmatrix}h (あなたあ、vあ、rあ）∂ h ( uあ、vあ、rあ)=あC私はR
100010
−Ａｐ _C私は
最小二乗問題は、ガウス-ニュートン アルゴリズムまたはレーベンバーグ-マルカートアルゴリズム を使用して解決できます。
6. カメラ測定のアップデート
6.1 透視投影（方位）測定モデル
いつか検討してくださいkカメラ画像から検出された 3D 特徴、画像平面上のuv uvuv (つまり、対応するピクセル座標) は次の式で与えられます:
zm , k = h ( xk ) + nk = hd ( zn , k , ζ ) + nk = hd ( hp ( C kpf ), ζ ) + nk = hd ( hp ( ht ( G pf , GG k R , G p C k ) ) , ζ ) + nk = hd ( hp ( ht ( ( hr ( λ , ⋯ ) ) , GG k R , G p C k ) , ζ ) + nk z_{m,k} = h(x_k) + n_k \\ =h_d(z_{n,k}, \zeta)+n_k \\ =h_d(h_p(^{C_k}p_f) , \zeta )+n_k\\ =h_d(h_p(h_t(^Gp_f,{^{G_k}_GR},^Gp_{C_k})), \zeta)+n_k\\ =h_d(h_p(h_t((h_r) (\lambda ,\cdots)),{^{G_k}_GR},^Gp_{C_k})), \zeta)+n_kzm 、 k=h ( xｋ）+nｋ=hd( zn 、 k、g )+nｋ=hd( hp(Cｋpふ）、g )+nｋ=hd( hp( hた(Gp _ふ、GGｋR 、GpCｋ)) 、g )+nｋ=hd( hp( hた(( hr( l 、⋯)) 、GGｋR 、GpCｋ)) 、g )+nｋ
どこだ、n_knｋは測定ノイズであり、通常はゼロ平均白色ガウスであると想定されます; zn , k z_{n,k}zn 、 k正規化された歪みのないUVです。紫外線観測値;ζ \zetaξは、焦点距離や歪みパラメータなどのカメラの固有パラメータです;C kpf ^{C_k}p_fCｋpふ現在のカメラ フレーム{ C k } \{C_k\}{ Cｋ}内の特徴位置G pf ^{G}p_fGp _ふはグローバル座標系{ G } \{G\}です{ G } ; { GG k R , G p C k } \{^{G_k}_GR,^Gp_{C_k}\}内の特徴の位置{ GGｋR 、GpCｋ} は、グローバル座標系 (またはカメラの外部パラメータ) における現在のカメラのポーズ (位置と向き) を表します;λ \lambdaλ は、単純な xyz 位置や方向を伴う逆深度など、さまざまな表現 (位置以外) の特性パラメーターです。
上の式では、測定関数をさまざまな操作に対応する複数のカスケード関数に分解し、状態を画像平面上の元のUV UVにマッピングします。UV測定中です。異なる特徴表現を使用して固有のキャリブレーションだけでなく外部キャリブレーションも実行するため、上記のカメラ測定モデルは普遍的であることに注意してください。次のセクションでは、各機能の概要を説明します。
測定機能の概要
| 関数 | 説明 |
|---|---|
| zk = hd ( zn , k , ζ ) z_k=h_d(z_{n,k},\zeta)zｋ=hd( zn 、 k、g ) | 正規化された座標を歪んだ UV 座標にマッピングするディストーション関数 |
| zn , k = hp ( C kpf ) z_{n,k}=h_p(^{C_k}p_f)zn 、 k=hp(Cｋpふ） | 画像内の 3D ポイントを取得し、それらを正規化された UV 座標に変換する投影機能 |
| C kp = ht ( G pf , GC k R , G p C k ) ^{C_k}p_=h_t(^Gp_f,^{C_k}_GR,^Gp_{C_k})Cｋp=hた(Gp _ふ、GCｋR 、GpCｋ） | グローバル座標系でのフィーチャの位置を現在のカメラ座標系に変換します |
| G pf = hr ( λ , ⋯ ) ^Gp_f = h_r(\lambda,\cdots)Gp _ふ=hr( l 、⋯） | グローバル座標系でのフィーチャ表現から 3D フィーチャへの変換 |
6.2 ヤコビアンの計算
上記の入れ子関数を考慮すると、連鎖規則を使用して状態の合計ヤコビアンを見つけることができます。特徴表現関数hr ( ⋯ ) h_r(\cdots)なのでhr( ⋯)状態、つまりカメラのポーズにも依存する可能性があり、その追加の導関数を注意深く考慮する必要があります。ヤコビのステータスxxについて次の点を考慮してください。xの測定例
∂ zk ∂ x = ∂ hd ( ⋅ ) ∂ zn , k ∂ hp ( ⋅ ) ∂ C kpf ∂ ht ( ⋅ ) ∂ x + ∂ hd ( ⋅ ) ∂ zn , k ∂ hp ( ⋅ ) ∂ C kpf ∂ht( ⋅ ) ∂ GP f ∂ hr ( ⋅ ) ∂ x {\partial z_k \over \partial x} = {\partial h_d(\cdot) \over \partial z_{n,k}} {\partial h_p(\cdot) \over \partial ^{C_k}p_f} {\partial h_t(\cdot) \over \partial x} +{\partial h_d(\cdot) \over \partial z_{n,k}} {\partial h_p(\ cdot) \over \partial ^{C_k}p_f} {\partial h_t(\cdot) \over \partial ^GP_f}{\partial h_r(\cdot) \over \partial x}∂ ×∂z _ｋ=∂z _n 、 k∂h _d( ⋅ )∂Cｋpふ∂h _p( ⋅ )∂ ×∂h _た( ⋅ )+∂z _n 、 k∂h _d( ⋅ )∂Cｋpふ∂h _p( ⋅ )∂GP _ふ∂h _た( ⋅ )∂ ×∂h _r( ⋅ )
グローバル フィーチャ表現 (「ポイント フィーチャ表現」セクションを参照) では 2 番目の項はゼロになりますが、カメラ表現ではこの項を計算する必要があります。
6.3 ディストーション関数
放射状歪み
カメラの固有機能を調整するには、正規化された座標を画像平面上の生のピクセル座標にマッピングする方法を知る必要があります。まず、OpenCV モデルで放射状歪みを使用します。
[ uv ] := zk = hd ( zn , k , ζ ) = [ fx ∗ x + cxfy ∗ y + cy ] \begin{bmatrix} u\\ v \\ \end{bmatrix} :=z_k=h_d(z_ {n,k},\zeta) = \begin{bmatrix} f_x*x+c_x \\ f_y*y+c_y \\ \end{bmatrix}[あなたv]:=zｋ=hd( zn 、 k、g )=[f×∗バツ+c×fはい∗y+cはい] x = xn ( 1 + k 1 r 2 + k 2 r 4 ) + 2 p 1 xnyn + p 2 ( r 2 + = 2 xn 2 ) y = yn ( 1 + k 1 r 2 + k 2 r 4 ) + 2 p 2 xnyn + p 1 ( r 2 + = 2 yn 2 ) r 2 = xn 2 + yn 2 x=x_n(1+k_1r^2+k_2r^4)+2p_1x_ny_n+p_2(r^2+=2x ^2_n) \\y=y_n(1+k_1r^2+k_2r^4)+2p_2x_ny_n+p_1(r^2+=2y^2_n)\\r^2=x^2_n+y^2_nバツ=バツん( 1+k1r2+k2r4 )+2P _1バツんyん+p2( r2+ _=2倍_n2）y=yん( 1+k1r2+k2r4 )+2P _2バツんyん+p1( r2+ _=2年n2）r2=バツn2+yn2
zn , k = [ xnyn ] ⊤ z_{n,k} = \begin{bmatrix} x_n & y_n \\ \end{bmatrix}^{\top}zn 、 k=[バツんyん]⊤は 3D フィーチャの正規化された座標です。uuu 和 v v vは、画像平面上の歪んだ画像座標です。上記の歪みモデルには、次の歪みパラメータとカメラ内 (焦点距離と画像中心) パラメータが含まれており、オンラインで推定できます: ζ =
[ fxfycxcyk 1 k 2 p 1 p 2 ] ⊤ \zeta = \begin{bmatrix} f_x & f_y & c_x & c_y & k_1 & k_2 &p_1 & p_2 \\ \end{bmatrix}^{\top}g=[f×fはいc×cはいk1k2p1p2]⊤
ほとんどのオフライン キャリブレーション方法 (Kalibr など) とは異なり、高次 (つまり 4 次以上) の項は推定しないことに注意してください。これらの内部パラメータ (歪みパラメータを含む) を推定するには、これらのパラメータのヤコビ行列が必要です:
∂ hd ( ⋅ ) ∂ ζ = [ fx ∗ ( r 2 + 2 xn 2 ) 0 y 0 1 fy ∗ ( ynr 2 ) fy ∗ ( ynr 4 ) fy ∗ ( r 2 + 2 yn 2 ) fy ∗ ( 2 xnyn ) ] {\partial h_d(\cdot ) \over \partial \zeta} = \begin{bmatrix} x & 0 & 1 & 0 & f_x*(x_nr^2) & f_x*(x_nr^4) & f_x*(2x_ny_n) & f_x*(r^2 +2x^2_n) \\0 & y & 0 & 1 & f_y*(y_nr^) 2) & f_y*(y_nr^4) & f_y*(r^2+2y^2_n) & f_y*(2x_ny_n) \end {bmatrix}∂ ζ∂h _d( ⋅ )=[バツ00はい1001f×∗( ×んr2 )fはい∗( yんr2 )f×∗( ×んr4 )fはい∗( yんr4 )f×∗( 2 ×んyん）fはい∗( r2+2年n2)f×∗( r2+2倍_n2）fはい∗( 2 ×んyん)]
同様に、正規化座標に対するヤコビアン座標は次のように取得できます。
∂ hd ( ⋅ ) ∂ zn , k {\partial h_d(\cdot) \over \partial z_{n,k}}∂z _n 、 k∂h _d( ⋅ )
魚眼モデル
魚眼レンズまたは広角レンズは実際に広く使用されているため、ここではOpenCV魚眼レンズにおけるこの歪みモデルの数学的導出を提供します。
[ uv ] : = zk = hd ( zn , k , ζ ) = [ fx ∗ x + cxfy ∗ y + cy ] \begin{bmatrix} u\\ v \\ \end{bmatrix} :=z_k=h_d(z_ {n,k},\zeta) = \begin{bmatrix} f_x*x+c_x \\ f_y*y+c_y \\ \end{bmatrix}[あなたv]:=zｋ=hd( zn 、 k、g )=[f×∗バツ+c×fはい∗y+cはい] x = xnr ∗ θ dy = ynr ∗ θ d θ d = θ ( 1 + k 1 θ 2 + k 2 θ 4 + k 3 θ 6 + k 4 θ 8 ) r 2 = xn 2 + yn 2 θ = の場合( r ) x={x_n \over r}*\theta_d\\y = {y_n\over r}*\theta_d\\\theta_d = \theta(1+k_1\theta^2+k_2\theta^4+k_3) \theta^6+k_4\theta^8) \\ r^2 = x^2_n+y^2_n \\ \theta=atan(r)バツ=rバツん∗私dy=ryん∗私d私d=私( 1+k1私2+k2私4+k3私6+k4私8 )r2=バツn2+yn2私=a t an ( r ) zn , k = [ xnyn ] ⊤ z_{n,k} = \begin{bmatrix} x_n & y_n \\ \end{bmatrix}^{\top
}zn 、 k=[バツんyん]⊤は 3D フィーチャの正規化された座標です。uuu 和 v v vは、画像平面上の歪んだ画像座標です。明らかに、上記のモデルでは次の歪み内部パラメータが使用されています:
ζ = [ fxfycxcyk 1 k 2 k 3 k 4 ] ⊤ \zeta = \begin{bmatrix} f_x & f_y & c_x & c_y & k_1 & k_2 &k_3 & k_4 \ \ \end {bmatrix}^{\top}g=[f×fはいc×cはいk1k2k3k4]⊤
方程式の滑らかな方程式の場合、方程式の方程式を決定することができます。
∂ hd ( ⋅ ) ∂ ζ = [ xn 0 1 0 fx ∗ ( xnr θ 3 ) fx ∗ ( xnr θ 5 ) ) fx ∗ ( xnr θ 7 ) ) fx ∗ ( xnr θ 9 ) ) 0 yn 0 1 fy ∗ ( ynr θ 3 ) ) fy ∗ ( ynr θ 5 ) ) fy ∗ ( ynr θ 7 ) ) fy ∗ ( ynr θ 9 ) ) { \partial h_d(\cdot)\over\partial\zeta} = \begin{bmatrix}x_n&0&1&0&f_x*({x_n\overr}\theta^3)&f_x*({x_n\ over r}\theta^5)) & f_x *({x_n\over r}\theta^7)) & f_x*({x_n\over r}\theta^9)) \\0 & y_n & 0 & & f_y*({y_n\over r}\theta ^3)) & f_y*({y_n\over r}\theta^5)) & f_y*({y_n\over r}\theta^7)) & f_y *({y_n\over r}\theta^9) ))\end{bmatrix}∂ ζ∂h _d( ⋅ )=[バツん00yん1001f×∗(rバツん私3 )fはい∗(ryん私3 ) )f×∗(rバツん私5 ))fはい∗(ryん私5 ) )f×∗(rバツん私7 ))fはい∗(ryん私7 ) )f×∗(rバツん私9 ))fはい∗(ryん私9 ) )]
同様に、微分連鎖規則を使用すると、正規化された座標に関して次のヤコビアン座標を計算できます:
∂ hd ( ⋅ ) ∂ zn , k = ∂ uv ∂ xy ∂ xy ∂ xnyn + ∂ uv ∂ xy ∂ xy ∂ r ∂ r ∂ xnyn + ∂ uv ∂ xy ∂ xy ∂ θ d ∂ θ d ∂ θ ∂ θ ∂ r ∂ r ∂ xnyn {\partial h_d(\cdot) \over \partial z_{n,k}} = {\partial uv \ over \partial xy} {\partial xy \over \partial x_ny_n} + {\partial uv \over \partial xy} {\partial xy \over \partial r} {\partial r \over \partial x_ny_n} + { \partial uv \over \partial xy} {\partial xy \over \partial \theta_d} {\partial \theta_d \over \partial \theta} {\partial \theta \over \partial r} {\partial r \over \partial x_ny_n }∂z _n 、 k∂h _d( ⋅ )=∂ x y∂紫外線∂ ×んyん∂ x y+∂ x y∂紫外線∂r _∂ x y∂ ×んyん∂r _+∂ x y∂紫外線∂θ _d∂ x y∂θ _∂θ _d∂r _∂θ _∂ ×んyん∂r _
6.4 透視投影機能
標準ピンホール カメラ モデルは、カメラ フレーム内の 3D 点を正規化された画像平面 (単位深度) に投影するために使用されます。 zn , k = hp (
C kpf ) = [ C x / C z C y / C z ] z_ { n,k} = h_p(^{C_k}p_f) = \begin{bmatrix} ^Cx/{^Cz}\\ ^Cy/{^Cz} \\ \end{bmatrix}zn 、 k=hp(Cｋpふ）=[C ×/Cz _Cy /_Cz _] C kpf = [ C x C y C z ] ^{C_k}p_f= \begin{bmatrix} ^Cx\\ ^Cy \\ {^Cz} \\ \end{bmatrix}Cｋpふ=
C ×Cy _Cz _
そのヤコビ行列は次のように計算されます:
∂ hp ( ⋅ ) ∂ C kpf = [ 1 C z 0 − C x ( C z ) 2 0 1 C z − C y ( C z ) 2 ] {\partial h_p( \cdot) \over \partial ^{C_k}p_f}= \begin{bmatrix} {1 \over {^Cz}} & 0 & {-^Cx\over (^Cz)^2}\\ 0 & {1 \over { ^Cz}} & {-^Cy \over (^Cz)^2} \\ \end{bmatrix}∂Cｋpふ∂h _p( ⋅ )=[Cz _100Cz _1(Cz )_2−Cx _(Cz )_2−Cy _]
6.5 ユークリッド変換
6DOF 剛体ユークリッド変換を使用して、現在のグローバル カメラのポーズに従ってグローバル座標系{ G } \{G\}を変換します。{
G }の 3D フィーチャの位置は、現在のカメラ座標系{ C k } \{C_k\}{
Cｋ}：
C kpf = ht ( G pf , GC k R , G p C k ) = GG k R ( G pf − G p C k ) ^{C_k}p_f = h_t(^Gp_f,{^{C_k}_GR} ,{^Gp_{C_k} }) = {^{G_k}_GR}({^Gp_f -{^Gp_{C_k}}})Cｋpふ=hた(Gp _ふ、GCｋR 、Gp _Cｋ）=GGｋR (Gp _ふ−Gp _Cｋ）
視覚慣性ナビゲーション システムでは、通常、カメラの状態ではなく IMU を状態ベクトルに保持することに注意してください。したがって、定数の IMU カメラ外部パラメータ{ICR , C p I } \{ {^C_IR}, {^Cp_I}\}を使用する必要があります。
{
私CR 、CP _私上記の幾何学的構造を次のようにさらに変換します。 G p C k = G p I k + IGRI p C k = G p I k + IGRI p C {^Gp_{C_k}}={^Gp_{I_k}}+ {
^G_IR^Ip_{C_k}} = {^Gp_{I_k}} +{^G_IR^Ip_C}Gp _Cｋ=Gp _私ｋ+私GR私はCｋ=Gp _私ｋ+私GR私はCGC k R = IC k RGI k R = ICRGI k R {^{C_k}_GR } = {^{C_k}_IR} {^{I_k}_GR} = {^{C}_IR} {^{I_k}_GR}GCｋR=私CｋRG私ｋR=私CRG私ｋR