참조 링크: OpenVINS https://docs.openvins.com/index.html
4. 측정 업데이트 파생
4.1 최소 평균 제곱 오차(MMSE) 추정
다음 정적 상태 추정 문제를 고려하십시오. 주어진 nnn 차원 가우스 랜덤 벡터x ∽ N ( x ^ ㊀ , P xx ㊀ ) x\backsim \mathcal{N}(\hat x^{㊀},P^{㊀}_{xx})엑스∽엔 (엑스^㊀ ,피더블 엑스㊀) 및 새로운mmm 차원 상태 독립적 영평균 백색 가우스 잡음n ∽ N ( 0 , R ) n\backsim \mathcal{N}(0,R)N∽엔 ( 0 ,R ) 적계Zm = z + n = h (x) + n z_m = z + n = h (x) + n지남=지+N=시간 ( x )+n , 후방 PDF의 처음 두 (중앙) 순간 p ( x ∣ zm ) p(x|z_m) 을계산하려고 합니다.p ( x ∣ z남) . 일반적으로(비선형 측정 모델이 주어지면) 사후 pdf를 다음과 같이 근사합니다:p ( x ∣ zm ) ≒ N ( x ^ ⊕ , P xx ⊕ ) p(x|z_m) \about \mathcal{N}(\ hat x^{\oplus},P^{\oplus}_{xx})p ( x ∣ z남)≒엔 (엑스^⊕ ,피더블 엑스⊕) . 설계상 이는 MMSE 추정 문제 Kay 1993에 대한 (대략적인) 솔루션입니다.
4.2 조건부 확률 분포
이를 위해 Bayes의 규칙을 사용합니다.
p ( x ∣ zm ) = p ( x , zm ) p ( zm ) p(x|z_m)={p(x,z_m) \over p(z_m ) }p ( x ∣ z남)=피 ( 지남)피 ( 엑스 ,지남)
일반적으로 이 조건부 PDF는 단순화된 가정을 적용하지 않고는 분석적으로 계산할 수 없습니다. 당면한 문제에 대해 먼저 p ( zm ) ≒ N ( z ^ , P zz ) p(z_m)\about \mathcal{N}(\hat{z},P_{zz}) 의 근사값을 구합니다.피 ( 지남)≒엔 (지^ ,피zz) (이것이 실제로 사실이라면) 다음과 같은 결합 가우스 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{배열} \right],\left[ \begin{배열}{ccc} P_{xx} & P_{xz} \\ P_{zx} & P_{zz}\\ \ end{배열} \right] \\ \end{pmatrix}=: \mathcal{N}(\hat y, P_{yy})피 ( 엑스 ,지남)=N([엑스^㊀z],[피xx피z x피xz _피zz])=:엔 (와이^,피yy)
이 두 가우스를 첫 번째 방정식으로 대체하여 다음과 같은 조건부 가우스 pdf를 얻습니다.
p ( x ∣ zm ) ≒ N ( y ^ , P yy ) N ( z ^ , P zz ) p(x|z_m) \about{ \mathcal{N}(\hat y,P_{yy} ) \over \ mathcal{N}(\hat z,P_{zz} )}p ( x ∣ z남)≒엔 (지^ ,피zz)엔 (와이^,피yy)= 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- \모자 z)]}=( 2p ) _n ∣피yy∣/∣ 피zz∣1이자형-21[( 와이 -와이^)⊤ 피응− 1( 와이 -와이^) − ( z남-지^ )⊤ 피zz− 1( z남-지^ )]= : N ( x ^ ⊕ , P xx ⊕ ) =:\mathcal{N}(\hat x^{\oplus},P^{\oplus}_{xx})=:엔 (엑스^⊕ ,피더블 엑스⊕)
이제 조건부 평균을 도출하고 공분산을 다음과 같이 계산할 수 있습니다. 먼저 분모 항∣ P yy ∣ / ∣ P zz ∣ |P_{yy}|/|P_{zz}|를∣ 피yy∣/∣ 피zz∣ P yy ∣ = ∣ [ P xx P xz P zx P zz ] ∣ = ∣ P xx − P xz P zz − 1 P zx ∣ ∣ P zz ∣ |P_{yy} |= \
∣ 피yy∣=
[피xx피z x피xz _피zz]
=
피xx-피xz _피zz− 1피z x
피zz
P zz P_{zz} 라고 가정합니다.피zz이는 가역적이며 Schur의 보수 의 결정적 특성을 사용합니다 . 따라서 다음과 같습니다:
∣ 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}∣ 피zz∣∣ 피yy∣=∣ 피zz∣
피xx-피xz _피zz− 1피z x
=
피xx-피xz _피zz− 1피z x
다음으로 오류 상태를 정의하여 rx = x − x ^ ㊀ r_x = x - \hat x^{㊀}아르 자형엑스=엑스-엑스^㊀，rz = zm − z ^ r_z = z_m -\hat z아르 자형z=지남-지^，ry = y − y ^ r_y = y - \hat 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)( 와-와이^)⊤ 피응− 1( 와-와이^)-( z,-지^ )⊤ 피zz− 1( 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=아르 자형와이⊤피응− 1아르 자형y-아르 자형지⊤피zz− 1아르 자형z= [ 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 z]⊤[피xx피z x피xz _피zz]- 1[아르 자형엑스r z]-아르 자형지⊤피zz− 1아르 자형z= ( 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엑스-피xz _피zz− 1아르 자형z)⊤ (피xx-피xz _피zz− 1피z x)− 1 (r엑스-피xz _피zz− 1아르 자형z)
여기서,( pzz − 1 ) ⊤ = P zz − 1 (p_{zz}^{-1} )^\top = P_{zz}^{-1}( 피zz− 1)⊤=피zz− 1공분산 행렬은 대칭이기 때문입니다. 지금까지 조건부 가우스 pdf를 다음과 같이 구성할 수 있습니다:
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}피 ( 엑스케이∣ z남)=( 2p ) _n ∣피xx-피xz _피zz− 1피z x∣1×전 x 피(-21[( r엑스-피xz _피zz− 1아르 자형z)⊤ (피xx-피xz _피zz− 1피z x)− 1 (r엑스-피xz _피zz− 1아르 자형z) ])
이는 우리가 찾고 있는 다음과 같은 조건부 평균과 공분산으로 이어집니다:
x ^ ⊕ = x ^ ㊀ + P xz P zz − 1 ( zm − z ^ ) \hat x^{\oplus} = \hat x^{㊀} + P_{xz}P_{zz}^{-1}(z_{m}-\hat z)엑스^⊕=엑스^㊀+피xz _피zz− 1( z남-지^ )P xx ⊕ = P xx ㊀ − P xz P zz − 1 P zx P_{xx}^{\oplus} = P_{xx}^{㊀}-P_{xz}P_{zz}^{-1} P_{zx}피더블 엑스⊕=피더블 엑스㊀-피xz _피zz− 1피z x
이는 (선형) 상태 추정을 위한 기본 방정식입니다.
4.3 선형 측정 업데이트
특별한 경우로 선형 MMSE 추정기를 설명하기 위해 간단한 선형 측정 모델을 고려합니다.
zm , k = H kxk + nk z_{m,k} = H_kx_k + n_k지m , k=시간케이엑스케이+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^{㊀}지^케이:=E [ zm , k]=이자 [ H케이엑스케이+N케이]=시간케이엑스^케이㊀
이를 통해 다음과 같이 공분산 및 상관 행렬을 도출할 수 있습니다.
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}]피zz=이자형 [( zm , k-지^케이) ( zm , k-지^케이)⊤ ]= 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]=전자 [( H케이엑스케이+N케이-시간케이엑스^케이㊀) ( H케이엑스케이+N케이-시간케이엑스^케이㊀)⊤ ]= H k P xx ㊀ H k ⊤ + R k =H_kP_{xx}^{㊀}H_{k}^\top+R_k=시간케이피더블 엑스㊀시간케이⊤+아르 자형케이
여기서 R k R_k아르 자형케이이산 측정 잡음 행렬, H k H_k시간케이상태를 측정 영역 P xx ㊀ P_{xx}^{㊀} 에 매핑하는 측정 야코비 행렬입니다.피더블 엑스㊀현재 상태 공분산입니다.
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}]피xz _=그리고 [( x케이-엑스^케이㊀) ( zm , k-지^케이)⊤ ]= 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케이-엑스^케이㊀) ( H케이엑스케이+N케이-시간케이엑스^케이㊀)⊤ ]= P xx ㊀ H k ⊤ =P_{xx}^{㊀}H_{k}^\top=피더블 엑스㊀시간케이⊤
우리는 잡음이 상태와 무관하다는 사실을 이용합니다. 이러한 양을 기본 방정식에 대체하면 다음과 같은 업데이트된 방정식이 제공됩니다
. 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)엑스^케이⊕=엑스^케이㊀+피케이㊀시간케이⊤( ㅎ케이피케이㊀시간케이⊤+아르 자형케이)− 1 (지m , k-지^케이) = x ^k ㊀ + K rz =\hat x_k^{㊀} +Kr_z=엑스^케이㊀+크르 _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)^\ 맨 위피더블 엑스⊕=피케이㊀+피케이㊀시간케이⊤( ㅎ케이피케이㊀시간케이⊤+아르 자형케이)- 1 (피케이㊀시간케이⊤)⊤ = 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^{㊀})=피케이㊀+피케이㊀시간케이⊤( ㅎ케이피케이㊀시간케이⊤+아르 자형케이)- 1 (H케이피케이㊀)
이는 본질적으로 칼만 필터(또는 선형 MMSE) 업데이트 방정식입니다.
5. 특징 삼각측량
5.1 3D 데카르트 삼각측량
우리는 특징의 3D 데카르트 위치를 초기에 추정할 수 있는 해결 가능한 선형 시스템을 만들고 싶습니다. 이를 위해 우리는 특징이 알려진 모든 포즈를 알려진 수량으로 처리합니다. 이러한 기능은 임의로 선택한 카메라 좌표계 { A } \{A\} 에 있을 수 있습니다.{ A } 아래에서 삼각 측량합니다특징점pf p_f피에프주어진 좌표계 { A } \{A\} 에 있을 수 있습니다.{ A } 아래의 포즈는1...m 1...m입니다.1... m 이 관찰되었습니다. 일부 카메라 포즈에서C i , i = 1... m C_i,i=1...m을씨나,나=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})씨나피에프=ㅏ씨나R (에이피 _에프-ㅏ피씨나) 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 ib ^{C_i}b 방향입니다.씨나b 와 깊이C iz ^{C_i}z씨나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{배열}{ccc} u_{n} \\ v_{n}\\ 1\\ \end{배열} \right]씨나피에프=씨나지에프씨나비에프=씨나지에프
유엔V엔1
우리는 u_n을 주목합니다유엔합 vn v_nV엔왜곡되지 않은 정규화된 이미지 좌표를 나타냅니다. 이 방향은 위 공식을 대체하여 고정 좌표계로 변환할 수 있습니다.
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 izf A b C i → f + A p C i =^{C_i}z_f{^Ab_{C_i\rightarrow f}}+^Ap_{C_i}=씨나지에프아비 _씨나→ 에프+ㅏ피씨나
추정된 깊이 C izf ^{C_i}z_f 를 제거하기 위해씨나지에프추가 자유도에 대한 요구를 충족하기 위해 방위각에 직교하는 다음 벡터 A b C i → f ^Ab_{C_i \rightarrow f}를 정의합니다.아비 _씨나→ 에프：
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[ \ 시작{배열}{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]앤 _나=⌊아비 _씨나→ 에프× ⌋=
0아비 _씨나→ 에프( 3 )-아비 _씨나→ 에프( 2 )-아비 _씨나→ 에프( 3 )0아비 _씨나→ 에프( 1 )아비 _씨나→ 에프( 2 )-아비 _씨나→ 에프( 1 )0
세 직선은 모두 벡터 A b C i → f ^Ab_{C_i \rightarrow f} 에 수직입니다.아비 _씨나→ 에프，因此AN i A b C i → f = 0 3 ^AN_i ^Ab_{C_i \rightarrow f}=0_3앤 _나에이비씨나→ 에프=03. 그런 다음 변환 방정식/제약 조건을 곱하여 알려지지 않은 방정식에만 관련된 두 개의 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}앤 _나에이피에프=ㅏN나씨나지에프아비 _씨나→ 에프+ㅏN나에이피씨나= AN i A p C i =^AN_i^Ap_{C_i}=ㅏN나에이피씨나
모든 측정값을 쌓아서 다음을 수행할 수 있습니다.
[ ⋮ AN i ⋮ ] ⏟ AA pf = [ ⋮ AN i A p C i ⋮ ] ⏟ b \underbrace{\left[ \begin{array}{ccc} \vdots \\ ^ AN_i\ \ \vdots\\ \end{배열} \right]}_{A}^A{p_f}=\underbrace{\left[ \begin{array}{ccc} \vdots \\ ^AN_i^Ap_{C_i }\ \ \vdots\\ \end{배열} \right]}_{b}ㅏ
⋮앤 _나⋮
피에프=비
⋮앤 _나에이피씨나⋮
각 픽셀 측정은 m > 1 m>1 인 한 두 가지 제약 조건을 제공하므로중>1 , 우리는 특징을 삼각 측량하기에 충분한 제약 조건을 갖습니다. 실제로 특징점의 뷰가 많을수록 삼각 측량이 더 잘되므로 일반적으로 최소 5개의 뷰에서 특징점을 확인하려고 합니다. 우리는 모든AN을 선택할 수 있습니다 ^AN_i앤 _나행 수를 줄이기 위해 두 행을 사용하지만 제곱 시스템을 사용하면 다음과 같은 "트릭"을 수행할 수 있습니다.
A ⊤ AA pf = A ⊤ b A^\top A ^Ap_f=A^\top bㅏ⊤ A에이피 _에프=ㅏ⊤b ( ∑ i A N i ⊤ A N i ) A p f = ( ∑ i A N i ⊤ A N i A p C i ) (\sum_i{^AN_i^\top {^AN_i}})^Ap_f=(\sum_i{
{^AN^\top _i{^AN_i}{^Ap_{C_i}}}}) (i∑ANi⊤ANi)Apf=(i∑ANi⊤ANiApCi)
这是一个 3x3 系统，与原始的 3mx3m 或 2mx2m 系统相比，可以快速解决。我们还检查三角测量特征是否“有效”，并且在相机前方且不太远。上述线性系统和否定系统的 condition number 对错误和极大值数据“敏感”。
5.2 1D 深度三角测量
我们希望创建一个可求解的线性系统，该系统可以让我们初步估计特征的 1D 深度位置。为此，我们将看到特征的所有姿势与锚架中的方向一起视为已知量。此功能将在我们可以任意选择的某个锚定摄像机帧 { A } \{A\} {
A} 中进行三角测量。我们将其定义为锚帧中的归一化图像坐标 [ u n v n 1 ] ⊤ \left[ \begin{array}{ccc} u_{n} &v_{n}&1\\ \end{array} \right]^\top [unvn1]⊤ . 앵커 포인트 포즈AAA , 기능이pf p_f피에프1...m 1...m 위치에 배치할 수 있습니다.1... m 관찰되면 모든 카메라 위치에서 관찰할 수 있습니다.C i , i = 1... m C_i,i=1...m씨나,나=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})씨나피에프=ㅏ씨나R (에이피 _에프-ㅏ피씨나) 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}ㅏ나피에프=ㅏ씨나아르 자형⊤씨나피에프+ㅏ피씨나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}z _에프에이피에프=ㅏ씨나아르 자형⊤씨나피에프+ㅏ피씨나
노이즈가 없는 경우 현재 좌표계의 측정은 C ib ^{C_i}b 방향입니다.씨나b 와 깊이C iz ^{C_i}z씨나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]씨나피에프=씨나지에프씨나비에프=씨나지에프
유엔V엔1
우리는 u_n을 주목합니다유엔합 vn v_nV엔왜곡되지 않은 정규화된 이미지 좌표를 나타냅니다. 이 방향은 위 공식을 대체하여 고정 좌표계로 변환할 수 있습니다.
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}z _에프아비 _에프=ㅏ씨나아르 자형⊤씨나지에프씨나비에프+ㅏ피씨나= C izf A b C i → f + A p C i =^{C_i}z_f{^Ab_{C_i\rightarrow f}}+^Ap_{C_i}=씨나지에프아비 _씨나→ 에프+ㅏ피씨나
추정된 깊이 C izf ^{C_i}z_f 를 제거하기 위해씨나지에프추가 자유도에 대한 요구를 충족하기 위해 방위각에 직교하는 다음 벡터 A b C i → f ^Ab_{C_i \rightarrow f}를 정의합니다.아비 _씨나→ 에프：
AN i = ⌊ A b C i → f × ⌋ ^AN_i=\lfloor {^Ab_{C_i \rightarrow f} \times} \rfloor앤 _나=⌊아비 _씨나→ 에프× ⌋
세 직선은 모두 벡터A b C i → f ^Ab_{C_i \rightarrow f}아비 _씨나→ 에프，因此AN i A b C i → f = 0 3 ^AN_i ^Ab_{C_i \rightarrow f}=0_3앤 _나에이비씨나→ 에프=03. 그런 다음 변환 방정식/제약 조건을 곱하여 알려지지 않은 방정식에만 관련된 두 개의 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}(앤 _나에이피에프)z _에프=ㅏN나씨나지에프아비 _씨나→ 에프+ㅏN나에이피씨나= AN i A p C i =^AN_i^Ap_{C_i}=ㅏN나에이피씨나
然后我们可以制定以下系窟：
(∑ i ( AN i A bf ) ⊤ ( AN i A bf ) ) )^\top ({^AN_i}}^Ab_f))^Az_f=(\sum_i{ {
(^AN^Ab_f)^\top _i{^AN_i}{^Ab_i}}})(나∑(앤 _나에이비에프)⊤ (앤 _나아비 _에프) )z _에프=(나∑(앤 _아비 _에프)나⊤앤 _나아비 _나)
단일 스칼라 분할로 빠르게 풀 수 있는 1x1 시스템입니다. 또한 삼각 측량 기능이 "유효"하고 카메라 앞에 있고 너무 멀지 않은지 확인합니다. 전체 특징은 다음과 같이 재구성할 수 있습니다.
A pf = A zf A bf ^Ap_f=^Az_f^Ab_f에이피 _에프=ㅏ지에프에이비에프
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})씨나피에프=ㅏ씨나R (에이피 _에프-ㅏ피씨나) = 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 {배열}{ccc} ^Ax_f/^Az_f \\^Ay_f/^Az_f \\ 1\\ \end{배열} \right] -{1\over {^Az_f}}^Ap_{C_i}\\ \end {pmatrix}=ㅏ지에프ㅏ씨나아르 자형
엑스 _에프/z _에프응 _에프/z _에프1
-z _에프1에이피 _씨나
我们定义u A = A xf / A zf u_A = ^Ax_f/^Az_f유에이=ㅏ엑스에프/z _에프， v A = A y f / A z f v_A= ^Ay_f/^Az_f vA=Ayf/Azf， ρ A = 1 / A z f \rho_A=1/^Az_f ρA=1/Azf 得到以下测量公式：
h ( u A , v A , ρ A ) = A C i R ( [ u A v A 1 ] − ρ A A 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(uA,vA,ρA)=ACiR
uAvA1
−ρAApCi
从相机坐标系看到的特征测量可以重新表述为：
z = [ u i v i ] z=\left[ \begin{array}{ccc} u_i\\ v_i \\ \end{array} \right] z=[uivi] = [ 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{array} \right] =[h(uA,vA,ρA)(1)/h(uA,vA,ρA)(3)h(uA,vA,ρA)(2)/h(uA,vA,ρA)(3)] = h ( u A , v A , ρ A ) =h(u_A,v_A,\rho_A) =h(uA,vA,ρA)
因此，我们可以制定最小二乘法和雅可比:
a r g m i n u A , v A , ρ A ∣ ∣ z − h ( u A , v A , ρ A ) ∣ ∣ 2 \underset{u_A,v_A,\rho_A}{argmin}||z-h(u_A,v_A,\rho_A)||^2 uA,vA,ρAargmin∣∣z−h(uA,vA,ρA)∣∣2 ∂ h ( u A , v A , ρ A ) h ( u A , v A , ρ A ) = A C 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{array} \right] -{^Ap_{C_i}}\\ \end{bmatrix} h(uA,vA,ρA)∂h(uA,vA,ρA)=ACiR
100010
−ApCi
6. Camera Measurement Update
6.1 透视投影（方位）测量模型
考虑在某个时间 k k k 从相机图像中检测到 3D 特征，其在图像平面上的测量值 u v uv uv（即相应的像素坐标）由下式给出：
z m , k = h ( x k ) + n k = h d ( z n , k , ζ ) + n k = h d ( h p ( C k p f ) , ζ ) + n k = h d ( h p ( h t ( G p f , G G k R , G p C k ) ) , ζ ) + n k = h d ( h p ( h t ( ( h r ( λ , ⋯ ) ) , G G k R , G p C k ) ) , ζ ) + n k 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_k zm,k=h(xk)+nk=hd(zn,k,ζ)+nk=hd(hp(Ckpf),ζ)+nk=hd(hp(ht(Gpf,GGkR,GpCk)),ζ)+nk=hd(hp(ht((hr(λ,⋯)),GGkR,GpCk)),ζ)+nk
其中 n k n_k nk 是测量噪声，通常假定为零均值白高斯; z n , k z_{n,k} zn,k 是归一化未畸变的 u v uv uv观测值; ζ \zeta ζ是相机的内在参数，如焦距和畸变参数; C k p f ^{C_k}p_f Ckpf是当前相机帧 { C k } \{C_k\} { Ck}中的特征位置; G p f ^{G}p_f Gpf是全局坐标系 { G } \{G\} { G}中的特征位置; { G G k R , G p C k } \{^{G_k}_GR,^Gp_{C_k}\} { GGkR,GpCk}表示全局坐标系中的当前相机姿势（位置和方向）（或相机外参）; λ \lambda λ是不同表示（位置除外）的特征参数，例如简单的 xyz 位置或带方位的逆深度。
在上面的表达式中，我们将测量函数分解为对应于不同操作的多个级联函数，这些函数将状态映射到图像平面上的原始 u v uv uv 测量中。应该注意的是，由于我们将执行具有不同特征表示的内参标定以及外参标定，因此上述相机测量模型是通用的。下一节将给出每个函数的高级说明。
测量函数概述
| Function | Description |
|---|---|
| z k = h d ( z n , k , ζ ) z_k=h_d(z_{n,k},\zeta) zk=hd(zn,k,ζ) | 将归一化坐标映射到畸变的 UV 坐标的畸变函数 |
| z n , k = h p ( C k p f ) z_{n,k}=h_p(^{C_k}p_f) zn,k=hp(Ckpf) | 获取图像中的 3D 点并将其转换为归一化 UV 坐标的投影函数 |
| C k p = h t ( G p f , G C k R , G p C k ) ^{C_k}p_=h_t(^Gp_f,^{C_k}_GR,^Gp_{C_k}) Ckp=ht(Gpf,GCkR,GpCk) | 将特征在全局坐标系中的位置转换为当前相机坐标系 |
| G p f = h r ( λ , ⋯ ) ^Gp_f = h_r(\lambda,\cdots) Gpf=hr(λ,⋯) | 在全局坐标系中从特征表示转换为 3D 特征 |
6.2 雅可比计算
给定上述嵌套函数，我们可以利用链式规则来找到总状态雅可比。由于我们的特征表示函数 h r ( ⋯ ) h_r(\cdots) hr(⋯) 也可能取决于状态，即相机姿态，我们需要仔细考虑其附加导数。考虑以下关于雅可比状态 x x x 的测量示例：
∂ z k ∂ x = ∂ h d ( ⋅ ) ∂ z n , k ∂ h p ( ⋅ ) ∂ C k p f ∂ h t ( ⋅ ) ∂ x + ∂ h d ( ⋅ ) ∂ z n , k ∂ h p ( ⋅ ) ∂ C k p f ∂ h t ( ⋅ ) ∂ G P f ∂ h r ( ⋅ ) ∂ 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} ∂x∂zk=∂zn,k∂hd(⋅)∂Ckpf∂hp(⋅)∂x∂ht(⋅)+∂zn,k∂hd(⋅)∂Ckpf∂hp(⋅)∂GPf∂ht(⋅)∂x∂hr(⋅)
在全局特征表达（请参阅点特征表达部分）中，第二项将为零，而对于相机表达，则需要计算该项。
6.3 畸变函数
径向畸变
为了校准相机内参，我们需要知道如何将归一化坐标映射到图像平面上的原始像素坐标。我们首先采用 OpenCV 模型中的径向畸变：
[ u v ] : = z k = h d ( z n , k , ζ ) = [ f x ∗ x + c x f y ∗ y + c y ] \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} [uv]:=zk=hd(zn,k,ζ)=[fx∗x+cxfy∗y+cy] x = x n ( 1 + k 1 r 2 + k 2 r 4 ) + 2 p 1 x n y n + p 2 ( r 2 + = 2 x n 2 ) y = y n ( 1 + k 1 r 2 + k 2 r 4 ) + 2 p 2 x n y n + p 1 ( r 2 + = 2 y n 2 ) r 2 = x n 2 + y n 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 x=xn(1+k1r2+k2r4)+2p1xnyn+p2(r2+=2xn2)y=yn(1+k1r2+k2r4)+2p2xnyn+p1(r2+=2yn2)r2=xn2+yn2
其中 z n , k = [ x n y n ] ⊤ z_{n,k} = \begin{bmatrix} x_n & y_n \\ \end{bmatrix}^{\top} zn,k=[xnyn]⊤ 是 3D 特征的归一化坐标， u u u 和 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=[에프엑스에프y씨엑스씨y케이1케이2피1피2]⊤
대부분의 오프라인 교정 방법(예: Kalibr)과 같이 고차(즉, 4차보다 높은) 항을 추정하지 않습니다. 이러한 내부 매개변수(왜곡 매개변수 포함)를 추정하려면 이러한 매개변수의 야코비 행렬이 필요합니다
. 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}∂ζ _∂ 시간디( ⋅ )=[엑스00y1001에프엑스*( x엔r2)fy∗(ynr2)fx∗(xnr4)fy∗(ynr4)fx∗(2xnyn)fy∗(r2+2yn2)fx∗(r2+2xn2)fy∗(2xnyn)]
类似地，关于归一化坐标的雅可比坐标可以按如下方式获得：
∂ h d ( ⋅ ) ∂ z n , k {\partial h_d(\cdot) \over \partial z_{n,k}} ∂zn,k∂hd(⋅)
鱼眼模型
由于鱼眼镜头或广角镜头在实践中被广泛使用，我们在这里提供了OpenCV鱼眼镜头中这种畸变模型的数学推导。
[ u v ] : = z k = h d ( z n , k , ζ ) = [ f x ∗ x + c x f y ∗ y + c y ] \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} [uv]:=zk=hd(zn,k,ζ)=[fx∗x+cxfy∗y+cy] x = x n r ∗ θ d y = y n r ∗ θ d θ d = θ ( 1 + k 1 θ 2 + k 2 θ 4 + k 3 θ 6 + k 4 θ 8 ) r 2 = x n 2 + y n 2 θ = a t a n ( 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) x=rx엔*나디와이=아르 자형와이엔*나디나디=나는 ( 1+케이1나2+케이2나4+케이3나6+케이4나8 )아르 자형2=엑스N2+와이N2나=a t an ( r ) zn , k = [ xnyn ] ⊤ z_{n,k} = \begin{bmatrix} x_n & y_n \\ \end{bmatrix}^{\top
}지n , k=[엑스엔와이엔]⊤ 는 3D 특징의 정규화된 좌표입니다.uuu 화vvv 는 이미지 평면의 왜곡된 이미지 좌표입니다. 분명히 위 모델에서는 다음과 같은 왜곡 내부 매개변수가 사용됩니다.
ζ = [ 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=[에프엑스에프y씨엑스씨y케이1케이2케이3케이4]⊤
매끄러운 방정식을 사용하는 매끄러운 방정식의 경우 방정식의 방정식을 결정하는 것이 가능합니다:
∂ 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}∂ζ _∂ 시간디( ⋅ )=[엑스엔00와이엔1001에프엑스*(아르 자형엑스엔나3 )에프y*(아르 자형와이엔나3 ) )에프엑스*(아르 자형엑스엔나5 ))에프y*(아르 자형와이엔나5 ) )에프엑스*(아르 자형엑스엔나7 ))에프y*(아르 자형와이엔나7 ) )에프엑스*(아르 자형엑스엔나9 ))에프y*(아르 자형와이엔나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 \ 부분 x_ny_n }∂ zn , k∂ 시간디( ⋅ )=∂ x y∂ 자외선∂ x엔와이엔∂ x y+∂ x y∂ 자외선∂r _∂ x y∂ x엔와이엔∂r _+∂ x y∂ 자외선∂ θ디∂ x y∂ θ∂ θ디∂r _∂ θ∂ x엔와이엔∂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}지n , k=시간피(씨케이피에프)=[C x/Cz _씨 y/Cz _] C kpf = [ C x C y C z ] ^{C_k}p_f= \begin{bmatrix} ^Cx\\ ^Cy \\ {^Cz} \\ \end{bmatrix}씨케이피에프=
C ×C yCz _
야코비 행렬은 다음과 같이 계산됩니다:
∂ 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}∂씨케이피에프∂ 시간피( ⋅ )=[Cz _100Cz _1(C z)2-Cx _(C z)2-C y]
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}}})씨케이피에프=시간티(Gp _에프,G씨케이R ,Gp _씨케이)=GG케이R (Gp _에프-Gp _씨케이)
시각 관성 항법 시스템에서는 일반적으로 상태 벡터의 카메라 상태가 아닌 IMU를 유지합니다. 따라서 우리는 상수 IMU 카메라 외부 매개변수 {ICR , C p I } \{ {^C_IR}, {^Cp_I}\}를 사용해야 합니다.
{
나씨R ,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 _씨케이=Gp _나케이+나지아르 자형나는 피씨케이=Gp _나케이+나지아르 자형나는 피씨GC k R = IC k RGI k R = ICRGI k R {^{C_k}_GR } = {^{C_k}_IR} {^{I_k}_GR} = {^{C}_IR} {^{I_k}_GR}G씨케이아르 자형=나씨케이아르 자형G나케이아르 자형=나씨아르 자형G나케이아르 자형