VSLAM學習(四) Bundle Adjustment

目錄

VSLAM學習(一) 三維運動、相機模型、SLAM模型

VSLAM學習(二) 非線性優化

VSLAM學習(三) 單目相機位姿估計

VSLAM學習(四) Bundle Adjustment

一、重投影誤差

1.1 誤差項

前面學過了，對于任意第i幀相機像素與空間點坐标的關系：

s i x i = K T P i 或 x i = 1 s i K T P i s_i\boldsymbol x_i=\boldsymbol K\boldsymbol T\boldsymbol P_i \quad\quad或\quad\quad \boldsymbol x_i=\frac{1}{s_i}\boldsymbol K\boldsymbol T\boldsymbol P_i si​xi​=KTPi​或xi​=si​1​KTPi​

但是有噪聲的存在，是以這個等式并不嚴格相等

我們記誤差

e i = x i − 1 s i K T P i \boldsymbol e_i=\boldsymbol x_i-\frac{1}{s_i}\boldsymbol K\boldsymbol T\boldsymbol P_i ei​=xi​−si​1​KTPi​

該誤差項稱為重投影誤差

因為 1 s i K T P i \frac{1}{s_i}\boldsymbol K\boldsymbol T\boldsymbol P_i si​1​KTPi​相當于是将世界坐标 P i \boldsymbol P_i Pi​再重新投影到相機中來的

1.2 誤差優化

對于一個重投影誤差項

e ( x ) = x − 1 s K T P \boldsymbol e(\boldsymbol x)=\boldsymbol x-\frac{1}{s}\boldsymbol K\boldsymbol T\boldsymbol P e(x)=x−s1​KTP

我們記

P ′ = ( T P ) 1 : 3 = R P + t = ( X ′ , Y ′ , Z ′ ) T \boldsymbol P'= (\boldsymbol T\boldsymbol P)_{1:3}= \boldsymbol R\boldsymbol P+\boldsymbol t= (X',Y',Z')^{\mathrm T} P′=(TP)1:3​=RP+t=(X′,Y′,Z′)T

再記

x r = 1 s K P ′ \boldsymbol x_r=\frac{1}{s}\boldsymbol K\boldsymbol P' xr​=s1​KP′

将 s x r = K P ′ s\boldsymbol x_r=\boldsymbol K\boldsymbol P' sxr​=KP′展開:

( s u r s v r s ) = ( f x 0 c x 0 f y c y 0 0 1 ) ( X ′ Y ′ Z ′ ) \begin{pmatrix} su_r \\ sv_r \\ s \end{pmatrix}= \begin{pmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{pmatrix} \begin{pmatrix} X' \\ Y' \\ Z' \end{pmatrix} ⎝⎛​sur​svr​s​⎠⎞​=⎝⎛​fx​00​0fy​0​cx​cy​1​⎠⎞​⎝⎛​X′Y′Z′​⎠⎞​

消去s得

{ u r = f x X ′ Z ′ + c x v r = f y Y ′ Z ′ + c y \left\{ \begin{aligned} u_r&=f_x\frac{X'}{Z'}+c_x \\ v_r&=f_y\frac{Y'}{Z'}+c_y \end{aligned} \right. ⎩⎪⎪⎨⎪⎪⎧​ur​vr​​=fx​Z′X′​+cx​=fy​Z′Y′​+cy​​

下面我們開始對誤差函數優化

對誤差函數做一階泰勒展開

e ( x + Δ x ) ≈ e ( x ) + J Δ x \boldsymbol e(\boldsymbol x+\Delta\boldsymbol x)≈\boldsymbol e(\boldsymbol x)+\boldsymbol J\Delta\boldsymbol x e(x+Δx)≈e(x)+JΔx

也就是考慮Jacobian矩陣 J \boldsymbol J J的形态

求出 J \boldsymbol J J的形态之後，便可以使用牛頓法或者LM方法求解極值即可。

1.2.1 優化位姿

現在我們的目标是優化 R , t \boldsymbol R,\boldsymbol t R,t，也就是優化 ξ \boldsymbol\xi ξ

根據擾動模型有

∂ e ∂ ξ = lim ⁡ δ ξ → 0 e ( δ ξ ⊕ ξ ) − e ( ξ ) δ ξ = ∂ e ∂ P ′ ∂ P ′ ∂ ξ \begin{aligned} \frac{\partial\boldsymbol e}{\partial\boldsymbol\xi}&= \lim\limits_{\delta\boldsymbol\xi\rightarrow 0} \frac{\boldsymbol e(\delta\boldsymbol\xi\oplus\boldsymbol\xi)-e(\boldsymbol\xi)}{\delta\boldsymbol\xi} \\ &=\frac{\partial\boldsymbol e}{\partial\boldsymbol P'} \frac{\partial\boldsymbol P'}{\partial\boldsymbol\xi} \end{aligned} ∂ξ∂e​​=δξ→0lim​δξe(δξ⊕ξ)−e(ξ)​=∂P′∂e​∂ξ∂P′​​

分别計算一下 ∂ e ∂ P ′ \displaystyle{\frac{\partial\boldsymbol e}{\partial\boldsymbol P'}} ∂P′∂e​和 ∂ P ′ ∂ ξ \displaystyle{\frac{\partial\boldsymbol P'}{\partial\boldsymbol\xi}} ∂ξ∂P′​

先來求第一項 ∂ e ∂ P ′ \displaystyle{\frac{\partial\boldsymbol e}{\partial\boldsymbol P'}} ∂P′∂e​

我們知道

e ( x ) = x − x r \boldsymbol e(\boldsymbol x)=\boldsymbol x-\boldsymbol x_r e(x)=x−xr​

其中 x \boldsymbol x x是從圖像上讀出來的像素（常數）， x r \boldsymbol x_r xr​是需要優化的量

故而

∂ e ∂ P ′ = − ∂ x r ∂ P ′ = − ( ∂ u r ∂ X ′ ∂ u r ∂ Y ′ ∂ u r ∂ Z ′ ∂ v r ∂ X ′ ∂ v r ∂ Y ′ ∂ v r ∂ Z ′ ) = − ( f x Z ′ 0 − f x X ′ Z ′ 2 0 f y Z ′ − f y Y ′ Z ′ 2 ) \frac{\partial\boldsymbol e}{\partial\boldsymbol P'}= -\frac{\partial\boldsymbol x_r}{\partial\boldsymbol P'}=- \begin{pmatrix} \displaystyle{\frac{\partial u_r}{\partial X'}} & \displaystyle{\frac{\partial u_r}{\partial Y'}} & \displaystyle{\frac{\partial u_r}{\partial Z'}} \\ \displaystyle{\frac{\partial v_r}{\partial X'}} & \displaystyle{\frac{\partial v_r}{\partial Y'}} & \displaystyle{\frac{\partial v_r}{\partial Z'}} \end{pmatrix}=- \begin{pmatrix} \displaystyle{\frac{f_x}{Z'}} & 0 & \displaystyle{-\frac{f_xX'}{Z'^2}} \\ 0 & \displaystyle{\frac{f_y}{Z'}} & \displaystyle{-\frac{f_yY'}{Z'^2}} \end{pmatrix} ∂P′∂e​=−∂P′∂xr​​=−⎝⎜⎛​∂X′∂ur​​∂X′∂vr​​​∂Y′∂ur​​∂Y′∂vr​​​∂Z′∂ur​​∂Z′∂vr​​​⎠⎟⎞​=−⎝⎜⎛​Z′fx​​0​0Z′fy​​​−Z′2fx​X′​−Z′2fy​Y′​​⎠⎟⎞​

對于第二項 ∂ P ′ ∂ ξ \displaystyle{\frac{\partial\boldsymbol P'}{\partial\boldsymbol\xi}} ∂ξ∂P′​，根據李代數擾動模型可知：

∂ P ′ ∂ ξ = ∂ ( T P ) 1 : 3 ∂ ξ = ( T P ) 1 : 3 ⊙ = ( I − P ′ ∧ 0 T 0 T ) 1 : 3 = ( I , − P ′ ∧ ) \frac{\partial\boldsymbol P'}{\partial\boldsymbol\xi}= \frac{\partial(\boldsymbol T\boldsymbol P)_{1:3}}{\partial\boldsymbol\xi}= (\boldsymbol T\boldsymbol P)_{1:3}^{\odot}= \begin{pmatrix} \boldsymbol I & -\boldsymbol P'^{\land} \\ \boldsymbol 0^{\mathrm T} & \boldsymbol 0^{\mathrm T} \end{pmatrix}_{1:3} =\big(\boldsymbol I, -\boldsymbol P'^{\land}\big) ∂ξ∂P′​=∂ξ∂(TP)1:3​​=(TP)1:3⊙​=(I0T​−P′∧0T​)1:3​=(I,−P′∧)

是以最終計算得

∂ e ∂ ξ = − ( f x Z ′ 0 − f x X ′ Z ′ 2 − f x X ′ Y ′ Z ′ 2 f x + f x X ′ 2 Z ′ 2 − f x Y ′ Z ′ 0 f y Z ′ − f y Y ′ Z ′ 2 − f y − f y Y ′ 2 Z ′ 2 f y X ′ Y ′ Z ′ 2 f y X ′ Z ′ ) \frac{\partial\boldsymbol e}{\partial\boldsymbol\xi}=- \begin{pmatrix} \displaystyle{\frac{f_x}{Z'}} & 0 & \displaystyle{-\frac{f_xX'}{Z'^2}} & \displaystyle{-\frac{f_xX'Y'}{Z'^2}} & \displaystyle{f_x+\frac{f_xX'^2}{Z'^2}} & \displaystyle{-\frac{f_xY'}{Z'}} \\ 0 & \displaystyle{\frac{f_y}{Z'}} & \displaystyle{-\frac{f_yY'}{Z'^2}} & \displaystyle{-f_y-\frac{f_yY'^2}{Z'^2}} & \displaystyle{\frac{f_yX'Y'}{Z'^2}} & \displaystyle{\frac{f_yX'}{Z'}} \end{pmatrix} ∂ξ∂e​=−⎝⎜⎛​Z′fx​​0​0Z′fy​​​−Z′2fx​X′​−Z′2fy​Y′​​−Z′2fx​X′Y′​−fy​−Z′2fy​Y′2​​fx​+Z′2fx​X′2​Z′2fy​X′Y′​​−Z′fx​Y′​Z′fy​X′​​⎠⎟⎞​

即為 J \boldsymbol J J的形态

1.2.2 優化空間點路标

現在我們的目标是優化空間點 P \boldsymbol P P

那麼我就這裡對 P \boldsymbol P P求導

∂ e ∂ P = ∂ e ∂ P ′ ∂ P ′ ∂ P \frac{\partial\boldsymbol e}{\partial\boldsymbol P}= \frac{\partial\boldsymbol e}{\partial\boldsymbol P'} \frac{\partial\boldsymbol P'}{\partial\boldsymbol P} ∂P∂e​=∂P′∂e​∂P∂P′​

由于

P ′ = R P + t \boldsymbol P'=\boldsymbol R\boldsymbol P+\boldsymbol t P′=RP+t

是以

∂ P ′ ∂ P = R \frac{\partial\boldsymbol P'}{\partial\boldsymbol P}=\boldsymbol R ∂P∂P′​=R

故而

∂ e ∂ P = − ( f x Z ′ 0 − f x X ′ Z ′ 2 0 f y Z ′ − f y Y ′ Z ′ 2 ) R \frac{\partial\boldsymbol e}{\partial\boldsymbol P}=- \begin{pmatrix} \displaystyle{\frac{f_x}{Z'}} & 0 & \displaystyle{-\frac{f_xX'}{Z'^2}} \\ 0 & \displaystyle{\frac{f_y}{Z'}} & \displaystyle{-\frac{f_yY'}{Z'^2}} \end{pmatrix} \boldsymbol R ∂P∂e​=−⎝⎜⎛​Z′fx​​0​0Z′fy​​​−Z′2fx​X′​−Z′2fy​Y′​​⎠⎟⎞​R

即為 J \boldsymbol J J的形态

二、光束平差法（Bundle Adjustment）批量優化

2.1 一階梯度的表示

回憶之前的SLAM模型

我們主要對觀測方程進行優化：

z i j = h ( ξ i , p j ) + v i j \boldsymbol z_{ij}=h(\boldsymbol \xi_i,\boldsymbol p_j)+\boldsymbol v_{ij} zij​=h(ξi​,pj​)+vij​

其中 z i j \boldsymbol z_{ij} zij​　是指 i i i時刻對第 j j j個路标點的觀測值，即拍攝圖檔上讀出的像素坐标

　　 ξ i \boldsymbol \xi_i ξi​　是指 i i i時刻的相機位姿

　　 p j \boldsymbol p_j pj​　表示第 j j j個路标點坐标

　　 v i j \boldsymbol v_{ij} vij​　表示 i i i時刻第 j j j個路标點的觀測噪聲

那麼可以定義誤差：

e i j = z i j − h ( ξ i , p j ) \boldsymbol e_{ij}=\boldsymbol z_{ij}-h(\boldsymbol \xi_i,\boldsymbol p_j) eij​=zij​−h(ξi​,pj​)

那麼代價函數就可以定義為：

1 2 ∑ i ∑ j ∥ e i j ∥ 2 = 1 2 ∑ i ∑ j ∥ z i j − h ( ξ i , p j ) ∥ 2 \frac{1}{2}\sum_i\sum_j\|\boldsymbol e_{ij}\|^2= \frac{1}{2}\sum_i\sum_j\|\boldsymbol z_{ij}-h(\boldsymbol \xi_i,\boldsymbol p_j)\|^2 21​i∑​j∑​∥eij​∥2=21​i∑​j∑​∥zij​−h(ξi​,pj​)∥2

為了計算最小值，我們可以将相機位姿 ξ \boldsymbol \xi ξ和路标 p \boldsymbol p p整合到一起

記

x = ( ξ 1 , ⋅ ⋅ ⋅ , ξ m , p 1 , ⋅ ⋅ ⋅ , p n ) T \boldsymbol x = (\boldsymbol \xi_1,···,\boldsymbol \xi_m,\boldsymbol p_1,···,\boldsymbol p_n)^{\mathrm T} x=(ξ1​,⋅⋅⋅,ξm​,p1​,⋅⋅⋅,pn​)T

那麼誤差函數 z i j − h ( ξ i , p j ) \boldsymbol z_{ij}-h(\boldsymbol \xi_i,\boldsymbol p_j) zij​−h(ξi​,pj​)就可以寫成 f ( x ) f(\boldsymbol x) f(x)

則

f ( x + Δ x ) ≈ ∑ i = 1 m ∑ j = 1 n ( e i j + ∑ k = 1 m ∂ e i j ∂ ξ k Δ ξ k + ∑ l = 1 n ∂ e i j ∂ p l Δ p l ) f(\boldsymbol x+\Delta\boldsymbol x)≈ \sum_{i=1}^m\sum_{j=1}^n(\boldsymbol e_{ij}+\sum_{k=1}^m\frac{\partial\boldsymbol e_{ij}}{\partial\boldsymbol\xi_k}\Delta\boldsymbol\xi_k+\sum_{l=1}^n\frac{\partial\boldsymbol e_{ij}}{\partial\boldsymbol p_l}\Delta\boldsymbol p_l) f(x+Δx)≈i=1∑m​j=1∑n​(eij​+k=1∑m​∂ξk​∂eij​​Δξk​+l=1∑n​∂pl​∂eij​​Δpl​)

其中 ∂ e i j ∂ ξ i , ∂ e i j ∂ p j \displaystyle{\frac{\partial\boldsymbol e_{ij}}{\partial\boldsymbol\xi_i}, \frac{\partial\boldsymbol e_{ij}}{\partial\boldsymbol p_j}} ∂ξi​∂eij​​,∂pj​∂eij​​的具體形式上面§1.2已經讨論過。

下面來推導

f ( x + Δ x ) ≈ f ( x ) + J Δ x f(\boldsymbol x+\Delta\boldsymbol x)≈ f(\boldsymbol x)+\boldsymbol J\Delta\boldsymbol x f(x+Δx)≈f(x)+JΔx

之中的 J \boldsymbol J J的形式

過程略繁瑣，我稍微寫得詳細點

記

x c = ( ξ 1 , ⋅ ⋅ ⋅ , ξ m ) T ∈ R 6 m x p = ( p 1 , ⋅ ⋅ ⋅ , p n ) T ∈ R 3 n F i j = ( ∂ e i j ∂ ξ 1 , ∂ e i j ∂ ξ 2 , ⋅ ⋅ ⋅ , ∂ e i j ∂ ξ m ) ∈ R 2 × 6 m E i j = ( ∂ e i j ∂ p 1 , ∂ e i j ∂ p 2 , ⋅ ⋅ ⋅ , ∂ e i j ∂ p n ) ∈ R 2 × 3 n \boldsymbol x_c = (\boldsymbol \xi_1,···,\boldsymbol \xi_m)^{\mathrm T} \in \mathbb R^{6m} \\ \boldsymbol x_p = (\boldsymbol p_1,···,\boldsymbol p_n)^{\mathrm T} \in \mathbb R^{3n} \\ \boldsymbol F_{ij}=(\frac{\partial\boldsymbol e_{ij}}{\partial\boldsymbol\xi_1},\frac{\partial\boldsymbol e_{ij}}{\partial\boldsymbol\xi_2},···,\frac{\partial\boldsymbol e_{ij}}{\partial\boldsymbol\xi_m}) \in\mathbb{R}^{2\times 6m} \\ \boldsymbol E_{ij}=(\frac{\partial\boldsymbol e_{ij}}{\partial\boldsymbol p_1},\frac{\partial\boldsymbol e_{ij}}{\partial\boldsymbol p_2},···,\frac{\partial\boldsymbol e_{ij}}{\partial\boldsymbol p_n}) \in\mathbb{R}^{2\times 3n} xc​=(ξ1​,⋅⋅⋅,ξm​)T∈R6mxp​=(p1​,⋅⋅⋅,pn​)T∈R3nFij​=(∂ξ1​∂eij​​,∂ξ2​∂eij​​,⋅⋅⋅,∂ξm​∂eij​​)∈R2×6mEij​=(∂p1​∂eij​​,∂p2​∂eij​​,⋅⋅⋅,∂pn​∂eij​​)∈R2×3n

那麼有

∑ k = 1 m ∂ e i j ∂ ξ k Δ ξ k = F i j Δ x c ∑ l = 1 n ∂ e i j ∂ p l Δ p l = E i j Δ x p \sum_{k=1}^m\frac{\partial\boldsymbol e_{ij}}{\partial\boldsymbol\xi_k}\Delta\boldsymbol\xi_k= \boldsymbol F_{ij}\Delta\boldsymbol x_c \\ \sum_{l=1}^n\frac{\partial\boldsymbol e_{ij}}{\partial\boldsymbol p_l}\Delta\boldsymbol p_l= \boldsymbol E_{ij}\Delta\boldsymbol x_p k=1∑m​∂ξk​∂eij​​Δξk​=Fij​Δxc​l=1∑n​∂pl​∂eij​​Δpl​=Eij​Δxp​

再記

J i j = ( F i j , E i j ) ∈ R 2 × ( 6 m + 3 n ) \boldsymbol J_{ij}=(\boldsymbol F_{ij},\boldsymbol E_{ij}) \in\mathbb{R}^{2\times (6m+3n)} Jij​=(Fij​,Eij​)∈R2×(6m+3n)

則有

F i j Δ x c + E i j Δ x p = J i j Δ x \boldsymbol F_{ij}\Delta\boldsymbol x_c+\boldsymbol E_{ij}\Delta\boldsymbol x_p= \boldsymbol J_{ij}\Delta\boldsymbol x Fij​Δxc​+Eij​Δxp​=Jij​Δx

繼續記

J = ( J 11 ⋮ J 1 n ⋮ J m 1 ⋮ J m n ) ∈ R 2 m n × ( 6 m + 3 n ) \boldsymbol J= \begin{pmatrix} \boldsymbol J_{11} \\ \vdots \\ \boldsymbol J_{1n} \\ \vdots \\ \boldsymbol J_{m1} \\ \vdots \\ \boldsymbol J_{mn} \end{pmatrix} \in\mathbb{R}^{2mn\times (6m+3n)} J=⎝⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎛​J11​⋮J1n​⋮Jm1​⋮Jmn​​⎠⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎞​∈R2mn×(6m+3n)

那麼

∑ i = 1 m ∑ j = 1 n J i j Δ x = J Δ x \sum_{i=1}^m\sum_{j=1}^n\boldsymbol J_{ij}\Delta\boldsymbol x=\boldsymbol J\Delta\boldsymbol x i=1∑m​j=1∑n​Jij​Δx=JΔx

這也就是整合過後的一階雅克比 J \boldsymbol J J的形式

2.2 稀疏性與邊緣化

在優化過程中，不論是使用高斯-牛頓法，還是使用LM方法，都會涉及到計算 J T J \boldsymbol J^{\mathrm T}\boldsymbol J JTJ的值

下面對 J \boldsymbol J J進行分析，看是否還能簡化。

注意矩陣 J i j \boldsymbol J_{ij} Jij​的實際意義，實際是在相機位姿 i i i檢視路标 j j j所産生的整體誤差

但不是所有的位姿 i i i中都會看見路标 j j j

如下圖所示

兩個相機位姿與4個路标點

其中相機 C 1 C_1 C1​沒有看到路标 p 4 p_4 p4​，相機 C 2 C_2 C2​沒有看到路标 p 1 p_1 p1​

是以

J 14 = J 21 = 0 \boldsymbol J_{14}=\boldsymbol J_{21}=\boldsymbol 0 J14​=J21​=0

故而整合的 J \boldsymbol J J可以省去這兩項，直接寫成

J = ( J 11 J 12 J 13 J 22 J 23 J 24 ) \boldsymbol J= \begin{pmatrix} \boldsymbol J_{11} \\ \boldsymbol J_{12} \\ \boldsymbol J_{13} \\ \boldsymbol J_{22} \\ \boldsymbol J_{23} \\ \boldsymbol J_{24} \end{pmatrix} J=⎝⎜⎜⎜⎜⎜⎜⎛​J11​J12​J13​J22​J23​J24​​⎠⎟⎟⎟⎟⎟⎟⎞​

不會影響 J Δ x \boldsymbol J\Delta\boldsymbol x JΔx和 J T J \boldsymbol J^{\mathrm T}\boldsymbol J JTJ的值

下面再來看看剩下不是 0 \boldsymbol 0 0的 J i j = ( F i j , E i j ) \boldsymbol J_{ij}=(\boldsymbol F_{ij},\boldsymbol E_{ij}) Jij​=(Fij​,Eij​)長什麼樣

其中

F i j = ( ∂ e i j ∂ ξ 1 , ⋅ ⋅ ⋅ , ∂ e i j ∂ ξ i , ⋅ ⋅ ⋅ , ∂ e i j ∂ ξ m ) \boldsymbol F_{ij}=(\frac{\partial\boldsymbol e_{ij}}{\partial\boldsymbol\xi_1},···,\frac{\partial\boldsymbol e_{ij}}{\partial\boldsymbol\xi_i},···,\frac{\partial\boldsymbol e_{ij}}{\partial\boldsymbol\xi_m}) Fij​=(∂ξ1​∂eij​​,⋅⋅⋅,∂ξi​∂eij​​,⋅⋅⋅,∂ξm​∂eij​​)

表示的是位姿 i i i和路标 j j j的誤差，對所有位姿的導數

而實際上這個誤差隻對位姿 i i i有影響，對其他位姿的誤差為 0 \boldsymbol 0 0

即 ∂ e i j ∂ ξ k ∣ k ≠ i = 0 \displaystyle{\frac{\partial\boldsymbol e_{ij}}{\partial\boldsymbol\xi_k}\Big|_{k≠i}=\boldsymbol 0} ∂ξk​∂eij​​∣∣∣​k​=i​=0，也就是

F i j = ( 0 2 × 6 , ⋅ ⋅ ⋅ , 0 2 × 6 , ∂ e i j ∂ ξ i , 0 2 × 6 , ⋅ ⋅ ⋅ , 0 2 × 6 ) \boldsymbol F_{ij}=(\boldsymbol 0_{2\times 6},···,\boldsymbol 0_{2\times 6},\frac{\partial\boldsymbol e_{ij}}{\partial\boldsymbol\xi_i},\boldsymbol 0_{2\times 6},···,\boldsymbol 0_{2\times 6}) Fij​=(02×6​,⋅⋅⋅,02×6​,∂ξi​∂eij​​,02×6​,⋅⋅⋅,02×6​)

同理，對于路标的誤差也有

E i j = ( 0 2 × 3 , ⋅ ⋅ ⋅ , 0 2 × 3 , ∂ e i j ∂ p j , 0 2 × 3 , ⋅ ⋅ ⋅ , 0 2 × 3 ) \boldsymbol E_{ij}=(\boldsymbol 0_{2\times 3},···,\boldsymbol 0_{2\times 3},\frac{\partial\boldsymbol e_{ij}}{\partial\boldsymbol p_j},\boldsymbol 0_{2\times 3},···,\boldsymbol 0_{2\times 3}) Eij​=(02×3​,⋅⋅⋅,02×3​,∂pj​∂eij​​,02×3​,⋅⋅⋅,02×3​)

是以整個 J i j \boldsymbol J_{ij} Jij​就隻會有兩個地方不是0

是以在使用高斯-牛頓法時的 J i j T J i j ∈ R ( 6 m + 3 n ) × ( 6 m + 3 n ) \boldsymbol J_{ij}^{\mathrm T}\boldsymbol J_{ij} \in\mathbb{R}^{(6m+3n)\times (6m+3n)} JijT​Jij​∈R(6m+3n)×(6m+3n)，其實隻有4個小塊矩陣的位置有值，其餘全為0

是以這個 H \boldsymbol H H矩陣

H = J T J = ∑ i = 1 m ∑ j = 1 n J i j T J i j = ( F T F F T E E T F E T E ) \boldsymbol H=\boldsymbol J^{\mathrm T}\boldsymbol J=\sum_{i=1}^m\sum_{j=1}^n\boldsymbol J_{ij}^{\mathrm T}\boldsymbol J_{ij}= \begin{pmatrix} \boldsymbol F^{\mathrm T}\boldsymbol F & \boldsymbol F^{\mathrm T}\boldsymbol E \\ \boldsymbol E^{\mathrm T}\boldsymbol F & \boldsymbol E^{\mathrm T}\boldsymbol E \end{pmatrix} H=JTJ=i=1∑m​j=1∑n​JijT​Jij​=(FTFETF​FTEETE​)

是個稀疏矩陣，大緻形式如下（一般相機位姿少，特征點、路标點較多）

我們将矩陣 H \boldsymbol H H分塊

顯然 B , C \boldsymbol B,\boldsymbol C B,C都是對角矩陣

是以，以高斯牛頓法為例，求解方程 H Δ x = g \boldsymbol H\Delta\boldsymbol x=\boldsymbol g HΔx=g就變成

( B A A T C ) ( Δ x c Δ x p ) = ( v w ) \begin{pmatrix} \boldsymbol B & \boldsymbol A \\ \boldsymbol A^{\mathrm T} & \boldsymbol C \end{pmatrix} \begin{pmatrix} \Delta\boldsymbol x_c \\ \Delta\boldsymbol x_p \end{pmatrix}= \begin{pmatrix} \boldsymbol v \\ \boldsymbol w \end{pmatrix} (BAT​AC​)(Δxc​Δxp​​)=(vw​)

下面使用Schur Elimination（Schur消元）法

将矩陣右上角的 A \boldsymbol A A消掉，即左乘矩陣 ( I − A C − 1 0 I ) \begin{pmatrix} \boldsymbol I & -\boldsymbol A\boldsymbol C^{-1} \\ \boldsymbol 0 & \boldsymbol I \end{pmatrix} (I0​−AC−1I​)

（消去左下角的 A T \boldsymbol A^{\mathrm T} AT去求 Δ x p \Delta\boldsymbol x_p Δxp​也行）

得：

( I − A C − 1 0 I ) ( B A A T C ) ( Δ x c Δ x p ) = ( I − A C − 1 0 I ) ( v w ) \begin{pmatrix} \boldsymbol I & -\boldsymbol A\boldsymbol C^{-1} \\ \boldsymbol 0 & \boldsymbol I \end{pmatrix} \begin{pmatrix} \boldsymbol B & \boldsymbol A \\ \boldsymbol A^{\mathrm T} & \boldsymbol C \end{pmatrix} \begin{pmatrix} \Delta\boldsymbol x_c \\ \Delta\boldsymbol x_p \end{pmatrix}= \begin{pmatrix} \boldsymbol I & -\boldsymbol A\boldsymbol C^{-1} \\ \boldsymbol 0 & \boldsymbol I \end{pmatrix} \begin{pmatrix} \boldsymbol v \\ \boldsymbol w \end{pmatrix} \\ (I0​−AC−1I​)(BAT​AC​)(Δxc​Δxp​​)=(I0​−AC−1I​)(vw​)

( B − A C − 1 A T 0 A T C ) ( Δ x c Δ x p ) = ( v − A C − 1 w w ) \begin{pmatrix} \boldsymbol B-\boldsymbol A\boldsymbol C^{-1}\boldsymbol A^{\mathrm T} & \boldsymbol 0 \\ \boldsymbol A^{\mathrm T} & \boldsymbol C \end{pmatrix} \begin{pmatrix} \Delta\boldsymbol x_c \\ \Delta\boldsymbol x_p \end{pmatrix}= \begin{pmatrix} \boldsymbol v-\boldsymbol A\boldsymbol C^{-1}\boldsymbol w \\ \boldsymbol w \end{pmatrix} (B−AC−1ATAT​0C​)(Δxc​Δxp​​)=(v−AC−1ww​)

是以

( B − A C − 1 A T ) Δ x c = v − A C − 1 w \Big(\boldsymbol B-\boldsymbol A\boldsymbol C^{-1}\boldsymbol A^{\mathrm T}\Big)\Delta\boldsymbol x_c=\boldsymbol v-\boldsymbol A\boldsymbol C^{-1}\boldsymbol w (B−AC−1AT)Δxc​=v−AC−1w

便可求出 Δ x c \Delta\boldsymbol x_c Δxc​，繼而求出 Δ x p \Delta\boldsymbol x_p Δxp​

這樣處理求解的原因是因為對角矩陣 C \boldsymbol C C的逆可以直接寫出，不需要複雜的計算。

至此就算出了一次疊代中的下降路線 Δ x \Delta\boldsymbol x Δx，之後便可以根據高斯牛頓或LM的原理繼續往下走了，進而完成優化。