BA優化的是最小二乘問題，那麼爲什麼可以使用最小二乘來估計呢？小二乘問題與概率之間的關係：
使用最大後驗概率（MAP）來得到狀態量，其中z是觀測值，x是狀態：x ^ = arg max x p ( x ∣ z ) (1)
\hat { \mathbf { x } } = \arg \max _ { \mathbf { x } } p ( \mathbf { x } |\mathbf { z } )\tag{1}
x ^ = arg x max p ( x ∣ z ) ( 1 )
x ^ = arg max x p ( x ∣ z ) = arg max x p ( z ∣ x ) p ( x ) p ( z ) = arg max x p ( z ∣ x ) p ( x )
\begin{aligned} \hat { \mathbf { x } } &= \arg \max _ { \mathbf { x } } p ( \mathbf { x } | \mathbf { z }) = \arg \max _ { \mathbf { x } } \frac { p ( \mathbf { z } | \mathbf { x } ) p ( \mathbf { x } ) } { p ( \mathbf { z } ) }\\
&= \arg \max _ { \mathbf { x } } p ( \mathbf { z } | \mathbf { x } ) p ( \mathbf { x } )
\end{aligned}
x ^ = arg x max p ( x ∣ z ) = arg x max p ( z ) p ( z ∣ x ) p ( x ) = arg x max p ( z ∣ x ) p ( x )
其中第一項化爲：p ( z ∣ x ) = ∏ k = 0 K p ( z k ∣ x k ) (2)
p ( \mathbf { z } | \mathbf { x } ) = \prod _ { k = 0 } ^ { K } p \left( \mathbf { z } _ { k } | \mathbf { x } _ { k } \right) \tag{2}
p ( z ∣ x ) = k = 0 ∏ K p ( z k ∣ x k ) ( 2 ) p ( x ) = p ( x 0 ∣ x ˇ 0 ) ∏ k = 1 K p ( x k ∣ x k − 1 ) (3)
p ( \mathbf { x } ) = p \left( \mathbf { x } _ { 0 } | \check { \mathbf { x } } _ { 0 } \right) \prod _ { k = 1 } ^ { K } p \left( \mathbf { x } _ { k } | \mathbf { x } _ { k - 1 } \right)\tag{3}
p ( x ) = p ( x 0 ∣ x ˇ 0 ) k = 1 ∏ K p ( x k ∣ x k − 1 ) ( 3 )
可以轉化爲最大似然估計（MLE）：x ^ = arg max x ∑ k = 0 K log p ( z k ∣ x k ) (4)
\hat { \mathbf { x } } = \arg \max _ { \mathbf { x } }\sum _ { k = 0 } ^ { K } \log p \left( \mathbf { z } _ { k } | \mathbf { x } _ { k } \right)\tag{4} x ^ = arg x max k = 0 ∑ K log p ( z k ∣ x k ) ( 4 ) log p ( z k ∣ x k ) = − 1 2 ( z k − z ( x k ) ) T Σ k − 1 ( z k − z ( x k ) ) − 1 2 log ( ( 2 π ) M det Σ k ) (5)
\log p \left( \mathbf { z } _ { k } | \mathbf { x } _ { k } \right) = - \frac { 1 } { 2 } \left( \mathbf { z } _ { k } - z(\mathbf { x } _ { k }) \right) ^ { T } \mathbf { \Sigma } _ { k } ^ { - 1 } \left( \mathbf { z } _ { k } - z(\mathbf { x } _ { k }) \right) - \frac { 1 } { 2 } \log \left( ( 2 \pi ) ^ { M } \operatorname { det } \mathbf { \Sigma } _ { k } \right)
\tag{5} log p ( z k ∣ x k ) = − 2 1 ( z k − z ( x k ) ) T Σ k − 1 ( z k − z ( x k ) ) − 2 1 log ( ( 2 π ) M d e t Σ k ) ( 5 ) x ^ = arg min x ∑ k = 0 K ∥ r ( z k , x k ) ∥ Σ k 2 (6)
\hat { \mathbf { x } } = \arg \min _ { \mathbf { x } }\sum _ { k = 0 } ^ { K }\|r(\mathbf{z}_k, \mathbf{x}_k)\|^2_{\Sigma_k} \tag{6}
x ^ = arg x min k = 0 ∑ K ∥ r ( z k , x k ) ∥ Σ k 2 ( 6 ) ∥ r ∥ Σ 2 = r T Σ − 1 r (7)
\|r\|^2_{\Sigma}=r^T\Sigma^{-1}r \tag{7}
∥ r ∥ Σ 2 = r T Σ − 1 r ( 7 )
最小二乘服從卡方分佈z = ( x − m ) T Σ − 1 ( x − m ) ∼ χ n 2 (8)
z = ( \mathbf { x } - \mathbf { m } ) ^ { T } \mathbf { \Sigma } ^ { - 1 } ( \mathbf { x } - \mathbf { m } ) \sim \chi _ { n } ^ { 2 }\tag{8}
z = ( x − m ) T Σ − 1 ( x − m ) ∼ χ n 2 ( 8 )
這相當於一種加權的最小二乘。
對於線性方程組，解的判別條件如下：
A x = 0 A{\bf x}={\bf 0} A x = 0
A m × n x = 0 A_{m\times n}{\bf x}={\bf 0} A m × n x = 0
當r ( A ) = n r(A)=n r ( A ) = n r ( A ) < n r(A)<n r ( A ) < n
A m × n x = b A_{m×n}{\bf x}={\bf b} A m × n x = b
當r ( A ) ≠ r ( A ∣ b ) r(A)≠r(A|b) r ( A ) = r ( A ∣ b ) r ( A ) = r ( A ∣ b ) = n r(A)=r(A|b)=n r ( A ) = r ( A ∣ b ) = n r ( A ) = r ( A ∣ b ) = r < n r(A)=r(A|b)=r<n r ( A ) = r ( A ∣ b ) = r < n
針對上面第三個方程，根據未知數和方程數量的關係可以分爲以下幾種情況：
如果m < n m<n m < n
如果m = n m=n m = n A A A x = A − 1 b x=A^{−1}b x = A − 1 b
如果m > n m>n m > n b \bf b b A A A
A x = b Ax=b A x = b
考慮m > n m>n m > n r ( A ) = n r(A)=n r ( A ) = n 奇異值分解 、正規方程 和QR分解 ，可以參考許可師兄的博客 。
我們要尋找的是使得∥ A x − b ∥ \|A {\bf x}-{\bf b} \| ∥ A x − b ∥ x {\bf x} x A A A A = ( a 1 , a 2 , . . . , a n ) A=(a_1,a_2,...,a_n) A = ( a 1 , a 2 , . . . , a n ) x {\bf x} x A x A{\bf x} A x A A A b {\bf b} b
針對這個子空間的理解：
假設A A A m × n m\times n m × n m > n m>n m > n
如圖，平面爲R a n k ( A ) Rank(A) R a n k ( A ) A x − b A{\bf x}-{\bf b} A x − b
因此讓向量A x − b A{\bf x}-{\bf b} A x − b A A A A T ( A x − b ) = 0 (9)
A^T(A{\bf x}-{\bf b})={\bf 0} \tag{9}
A T ( A x − b ) = 0 ( 9 ) A T A x = A T b x = ( A T A ) − 1 A T b (10)
A^TA{\bf x}=A^T{\bf b} \\
{\bf x}=(A^TA)^{-1}A^T{\bf b} \tag{10}
A T A x = A T b x = ( A T A ) − 1 A T b ( 1 0 )
爲什麼有解，因爲A T b A^Tb A T b A T A A^TA A T A
加權的最小二乘問題：( A T C A ) x = A T C b (11)
\left( \mathrm { A } ^ { \mathrm { T } } \mathrm { CA } \right) \mathbf { x } = \mathrm { A } ^ { \mathrm { T } } \mathrm { Cb } \tag{11}
( A T C A ) x = A T C b ( 1 1 )
考慮A ∈ R n × n A\in R^{n\times n} A ∈ R n × n r ( A ) = n r(A)=n r ( A ) = n x = A − 1 b (12)
{\bf x=A^{-1}b} \tag{12}
x = A − 1 b ( 1 2 )
通常方便求解的一些特殊係數矩陣 包括：
對角矩陣：直接可以求出。
上三角矩陣：從最後一個未知數開始解，使用後向帶入（back-substitution）的方式。
下三角矩陣：從第一個未知數開始解，使用前向帶入（forward-substitution）的方式。
正交矩陣：轉置求解，特殊的有Householder矩陣A = I − 2 u u T A=I-2uu^T A = I − 2 u u T
排列矩陣：實現矩陣的行交換和列交換的矩陣，只有1或者0。直接求解。
矩陣的求解方法可以分爲：
因式分解的方法，把係數矩陣A分解成上面所描述的一些方便求解的矩陣的連乘形式進行求解。
分塊消元的方法，對係數矩陣具有特殊結構的形式，可以進行分塊消元來求解。
針對非奇異矩陣 ，可以因式分解爲A = P L U (13)
A=PLU \tag{13}
A = P L U ( 1 3 )
帶狀結構矩陣和稀疏矩陣可以獲得更快的運算。對於稀疏矩陣有如下分解：A = P 1 L U P 2 (14)
A=P_1LUP_2 \tag{14}
A = P 1 L U P 2 ( 1 4 ) P 1 P 2 P_1P_2 P 1 P 2 非零元素的個數 、稀疏模式和使用的算法等。
針對對稱正定矩陣 ，可以因式分解爲A = L L T (15)
A=LL^T \tag{15}
A = L L T ( 1 5 )
對於稀疏矩陣，有分解形式：A = P L L T P T (16)
A=PLL^TP^T \tag{16}
A = P L L T P T ( 1 6 )
針對非奇異對稱矩陣 ，可以分解爲A = P L D L T P T (17)
A=PLDL^TP^T \tag{17}
A = P L D L T P T ( 1 7 )
這種方法對於子矩陣容易因式分解的A具有較好的效果，如塊對角和帶狀矩陣。將A x = b Ax=b A x = b
[ A 11 A 12 A 21 A 22 ] [ x 1 x 2 ] = [ b 1 b 2 ] (18)
\begin{bmatrix}
A_{11}&A_{12} \\
A_{21}&A_{22}
\end{bmatrix}
\begin{bmatrix}
x_1 \\ x_2
\end{bmatrix} =
\begin{bmatrix}
b_1 \\ b_2
\end{bmatrix} \tag{18}
[ A 1 1 A 2 1 A 1 2 A 2 2 ] [ x 1 x 2 ] = [ b 1 b 2 ] ( 1 8 )
若A 11 A_{11} A 1 1 x 1 = A 11 − 1 ( b 1 − A 12 x 2 ) (19)
x_1=A_{11}^{-1}(b_1-A_{12}x_2) \tag{19}
x 1 = A 1 1 − 1 ( b 1 − A 1 2 x 2 ) ( 1 9 ) ( A 22 − A 21 A 11 − 1 A 12 ) x 2 = b 2 − A 21 A 11 − 1 b 1 (20)
(A_{22}-A_{21}A_{11}^{-1} A_{12})x_2=b_2-A_{21}A_{11}^{-1}b_1 \tag{20}
( A 2 2 − A 2 1 A 1 1 − 1 A 1 2 ) x 2 = b 2 − A 2 1 A 1 1 − 1 b 1 ( 2 0 ) S = A 22 − A 21 A 11 − 1 A 12 (21)
S=A_{22}-A_{21}A_{11}^{-1}A_{12} \tag{21}
S = A 2 2 − A 2 1 A 1 1 − 1 A 1 2 ( 2 1 ) A 11 A_{11} A 1 1 Schur補 ，當且僅當A非奇異時Schur補非奇異。
這種消元可以看做是分塊的LU分解，現在採用的因式分解求解方法都是採用分塊因式分解和求解的步驟。求解公式（19）和公式（20）過程中都需要對A 11 A_{11} A 1 1 S S S
當A 11 A_{11} A 1 1 A 11 A_{11} A 1 1
若A 11 A_{11} A 1 1 A 11 A_{11} A 1 1 A 11 A_{11} A 1 1 用分塊消元法求解箭式結構的方程遠比標準方法有效 ，這就是SLAM求解正規方程採用這種方法的原因。當A是對稱 或對稱正定 矩陣時，A 11 A_{11} A 1 1 S S S
高斯牛頓法（Gauss-Newton）就不多介紹了。推導一下它的收斂性，是以二次來近似的一種思想。
設最小二乘形式爲：G ( x ) = 1 2 ∥ F ( x ) ∥ 2 (22)
G(x)=\frac{1}{2}\|F(x)\|^2 \tag{22}
G ( x ) = 2 1 ∥ F ( x ) ∥ 2 ( 2 2 ) G ( x ) G(x) G ( x )
G ( x k + 1 ) = G ( x k ) + ∂ G ∂ x Δ x + 1 2 Δ x T ∂ 2 G ∂ x 2 Δ x = G ( x k ) + F ( x k ) ∂ F ∂ x Δ x + 1 2 Δ x T ( ∂ F ∂ x T ∂ F ∂ x + F ( x k ) ∂ 2 F ∂ x 2 ) Δ x (23)
\begin{aligned}
G(x_{k+1}) &= G(x_k)+\frac{\partial G}{\partial x}\Delta x+\frac{1}{2}\Delta x^T\frac{\partial^2 G}{\ \partial x^2} \Delta x \\&=G(x_k)+F(x_k)\frac{\partial F}{\partial x}\Delta x+\frac{1}{2}\Delta x^T\left(\frac{\partial F}{\partial x}^T\frac{\partial F}{\partial x}+F(x_k)\frac{\partial^2 F}{\partial x^2}\right)\Delta x
\end{aligned} \tag{23}
G ( x k + 1 ) = G ( x k ) + ∂ x ∂ G Δ x + 2 1 Δ x T ∂ x 2 ∂ 2 G Δ x = G ( x k ) + F ( x k ) ∂ x ∂ F Δ x + 2 1 Δ x T ( ∂ x ∂ F T ∂ x ∂ F + F ( x k ) ∂ x 2 ∂ 2 F ) Δ x ( 2 3 )
G-N方法中：
對F ( x ) F(x) F ( x ) F ( x k + 1 ) = F ( x k ) + ∂ F ∂ x Δ x (24)
F(x_{k+1}) =F(x_k)+\frac{\partial F}{\partial x}\Delta x \tag{24}
F ( x k + 1 ) = F ( x k ) + ∂ x ∂ F Δ x ( 2 4 ) G ( x k + 1 ) = 1 2 F 2 ( x k ) + F ( x k ) ∂ F ∂ x Δ x + 1 2 Δ x T ∂ F ∂ x T ∂ F ∂ x Δ x (25)
G(x_{k+1}) = \frac{1}{2}F^2(x_k)+F(x_k)\frac{\partial F}{\partial x}\Delta x+\frac{1}{2}\Delta x^T\frac{\partial F}{\partial x}^T\frac{\partial F}{\partial x}\Delta x \tag{25}
G ( x k + 1 ) = 2 1 F 2 ( x k ) + F ( x k ) ∂ x ∂ F Δ x + 2 1 Δ x T ∂ x ∂ F T ∂ x ∂ F Δ x ( 2 5 ) S ( x ∗ ) = F ( x ∗ ) ∂ 2 F ∂ x 2 (26)
S(x^*)=F(x^*)\frac{\partial^2 F}{\partial x^2} \tag{26}
S ( x ∗ ) = F ( x ∗ ) ∂ x 2 ∂ 2 F ( 2 6 ) J = ∂ F ∂ x H = ∂ 2 F ∂ x 2 (27)
J=\frac{\partial F}{\partial x} \qquad H=\frac{\partial^2 F}{\partial x^2} \tag{27}
J = ∂ x ∂ F H = ∂ x 2 ∂ 2 F ( 2 7 ) x k → x ∗ x_k \rightarrow x^* x k → x ∗
二階收斂：當多餘的項∥ S ( x ∗ ) ∥ = 0 \|S(x^*)\|=0 ∥ S ( x ∗ ) ∥ = 0 F ( x ∗ ) = 0 F(x^*)=0 F ( x ∗ ) = 0 H ( x ∗ ) = 0 H(x^*)=0 H ( x ∗ ) = 0
線性收斂：當∥ S ( x ∗ ) ∥ ≠ 0 \|S(x^*)\|\neq 0 ∥ S ( x ∗ ) ∥ = 0 ∥ S ( x ∗ ) ∥ \|S(x^*)\| ∥ S ( x ∗ ) ∥
∥ S ( x ∗ ) ∥ \|S(x^*)\| ∥ S ( x ∗ ) ∥
補充：
Newton法的二階收斂性，初始點充分靠近極值點，並且Hessian矩陣非奇異，並且Hessian矩陣在極值點附近Lipschitz連續。
注：Lipschitz連續是一種更強的連續，限制函數的改變速度。∥ G k − 1 ∥ ≤ M \|G_k^{-1}\|\leq M ∥ G k − 1 ∥ ≤ M
H H H J J J H H H ( J T J ) − 1 J T (J^TJ)^{-1}J^T ( J T J ) − 1 J T
當函數梯度很大時，收斂速度慢
依賴於H矩陣，奇異時算法失效
非線性程度強時算法失效，如上面所示；
舉例
無人機做IMU融合時候，加速度計懸停和勻速運動都會導致H k H_k H k
對公式（22）進行一階展開，求得負梯度爲最速下降方向，在進行直線搜索得到最優步長。注意和GN的展開對象不同 。
解決J k T J k {J^T_k} J_k J k T J k
將公式（24）帶入公式（22）中，則trust-region的最小二乘可以表示爲：min d 1 2 ∥ F ( x ) + J Δ x ∥ 2 s . t . ∥ Δ x ∥ 2 ≤ ∥ Δ k ∥ 2 (28)
\min_d \frac{1}{2}\|F(x)+J\Delta x\|^2 \\
s.t. \quad \|\Delta x\|^2\leq\|\Delta_k\|^2 \tag{28}
d min 2 1 ∥ F ( x ) + J Δ x ∥ 2 s . t . ∥ Δ x ∥ 2 ≤ ∥ Δ k ∥ 2 ( 2 8 ) J T ( F ( x ) + J Δ x ) + λ Δ x = 0 λ ( ∥ Δ k ∥ 2 − ∥ Δ x ∥ 2 ) = 0 (29)
J^T(F(x)+J\Delta x)+\lambda \Delta x=0 \\
\lambda(\|\Delta_k\|^2-\|\Delta x\|^2)=0 \tag{29}
J T ( F ( x ) + J Δ x ) + λ Δ x = 0 λ ( ∥ Δ k ∥ 2 − ∥ Δ x ∥ 2 ) = 0 ( 2 9 ) ( J T J + λ I ) Δ x = − J T F ( x ) (30)
(J^TJ+\lambda I)\Delta x = -J^TF(x) \tag{30}
( J T J + λ I ) Δ x = − J T F ( x ) ( 3 0 )
設公式（28）得到q k ( Δ x ) = 1 2 ∥ F ( x ) + J Δ x ∥ 2 q_k(\Delta x)=\frac{1}{2}\|F(x)+J\Delta x\|^2 q k ( Δ x ) = 2 1 ∥ F ( x ) + J Δ x ∥ 2
根據公式（22）設：KaTeX parse error: Undefined control sequence: \
at position 97: … k } \right) \\\̲
̲\begin{aligned}… ρ k = Δ G k Δ q k (32)
\rho _ { k } = \frac { \Delta G_ { k } } { \Delta q _ { k } }\tag{32}
ρ k = Δ q k Δ G k ( 3 2 ) λ \lambda λ τ \tau τ 1 0 − 6 10^{-6} 1 0 − 6 1 0 − 3 , 1 10^{-3}, 1 1 0 − 3 , 1
阻尼同時受到下降方向和步驟的多少，隨着步驟增加應該減小。開始較遠使用最速下降，接近後使用高斯牛頓二階收斂快。
代碼中LM一般的作用只有兩個，一個是防止Hessian不滿秩，一個是當GN失敗的時候增加lambda來找到下降方向。開始lambda不應該設置太大，會導致收斂慢（最速下降沒有GN收斂性好）。也可以增加scale來保證delta大小合適，如同dso。最速下降沒有GN效果好。
SVO中使用LM計算稀疏圖像對齊的方法如下：
設置μ \mu μ
double H_max_diag = 0;
double tau = 1e-4;
H_max_diag = max(H_max_diag, fabs(H_(j,j))); //j=1.2....6
mu_ = tau*H_max_diag;
計算H H H
H_ += (H_.diagonal()*mu_).asDiagonal();
求解最小二乘問題，成功則更新優化變量，計算新的誤差。誤差比上一次迭代減小則轉4 ，求解失敗或者誤差增大則轉5 .
迭代成功，判斷終止條件，並且更新參數
stop_ = vk::norm_max(x_)<=eps_; //判斷增量足夠小
mu_ *= max(1./3., min(1.-pow(2*rho_-1,3), 2./3.)); //縮小mu_，還是很保守的，沒有增大
nu_ = 2.;
迭代失敗，增大置信區間，嘗試幾次。
mu_ *= nu_;
nu_ *= 2.;
++n_trials_;
總結，就是error減小了，則減小μ \mu μ μ \mu μ
LM算法中λ \lambda λ
求解GN方程得到步長∥ Δ G N x ∥ ≤ Δ k \|\Delta^{GN} x\| \leq \Delta_k ∥ Δ G N x ∥ ≤ Δ k
根據最速下降法直線搜索得到最優步長α k \alpha_k α k α k ∥ Δ S D x ∥ ≥ Δ k \alpha_k\|\Delta^{SD}x\| \geq \Delta_k α k ∥ Δ S D x ∥ ≥ Δ k Δ k Δ S D x ∥ Δ S D x ∥ \Delta _k \frac{\Delta^{SD}x}{\|\Delta^{SD}x\|} Δ k ∥ Δ S D x ∥ Δ S D x Δ k \Delta_k Δ k
否則在二者所張成的空間內選擇一個方向使得下降的長度爲Δ k \Delta _k Δ k
Δ x = α k Δ S D x + ( Δ G N x − α k Δ S D x ) β (33)
\Delta x=\alpha_k\Delta^{SD}x+(\Delta^{GN}x-\alpha_k\Delta^{SD}x)\beta \tag{33}
Δ x = α k Δ S D x + ( Δ G N x − α k Δ S D x ) β ( 3 3 )
得到下降的方向與步長，其中β \beta β ∥ Δ x ∥ = Δ k \|\Delta x\|=\Delta _k ∥ Δ x ∥ = Δ k
同樣Dog-Leg可以應對GN法中Hessian矩陣奇異的情況，使用最速下降法來補償，而LM算法卻保證了Hessian永遠是非奇異的。一般認爲Dog-Leg比LM性能更好，並且能達到相同的精度。
滿足u T A v = 0 \mathbf{u}^{\mathrm{T}} \mathbf{A} \mathbf{v}=0 u T A v = 0 A \mathbf A A
設二次函數f ( x ) = 1 2 x T A x − x T b
f(x)=\frac{1}{2} \mathbf{x}^{\mathrm{T}} \mathbf{A} \mathbf{x}-\mathbf{x}^{\mathrm{T}} \mathbf{b}
f ( x ) = 2 1 x T A x − x T b A x − b \mathbf{A x}-\mathbf{b} A x − b A \mathbf A A
算法如下：
r 0 : = b − A x 0 if r 0 is sufficiently small, then return x 0 as the result p 0 : = r 0 k : = 0 repeat α k : = r k T r k p k T A p k x k + 1 : = x k + α k p k r k + 1 : = r k − α k A p k if r k + 1 is sufficiently small, then exit loop β k : = r k + 1 T r k + 1 r k T r k p k + 1 : = r k + 1 + β k p k k : = k + 1 end repeat return x k + 1 as the result {\displaystyle {\begin{aligned}&\mathbf {r} _{0}:=\mathbf {b} -\mathbf {Ax} _{0}\\&{\text{if }}\mathbf {r} _{0}{\text{ is sufficiently small, then return }}\mathbf {x} _{0}{\text{ as the result}}\\&\mathbf {p} _{0}:=\mathbf {r} _{0}\\&k:=0\\&{\text{repeat}}\\&\qquad \alpha _{k}:={\frac {\mathbf {r} _{k}^{\mathsf {T}}\mathbf {r} _{k}}{\mathbf {p} _{k}^{\mathsf {T}}\mathbf {Ap} _{k}}}\\&\qquad \mathbf {x} _{k+1}:=\mathbf {x} _{k}+\alpha _{k}\mathbf {p} _{k}\\&\qquad \mathbf {r} _{k+1}:=\mathbf {r} _{k}-\alpha _{k}\mathbf {Ap} _{k}\\&\qquad {\text{if }}\mathbf {r} _{k+1}{\text{ is sufficiently small, then exit loop}}\\&\qquad \beta _{k}:={\frac {\mathbf {r} _{k+1}^{\mathsf {T}}\mathbf {r} _{k+1}}{\mathbf {r} _{k}^{\mathsf {T}}\mathbf {r} _{k}}}\\&\qquad \mathbf {p} _{k+1}:=\mathbf {r} _{k+1}+\beta _{k}\mathbf {p} _{k}\\&\qquad k:=k+1\\&{\text{end repeat}}\\&{\text{return }}\mathbf {x} _{k+1}{\text{ as the result}}\end{aligned}}} r 0 : = b − A x 0 if r 0 is sufficiently small, then return x 0 as the result p 0 : = r 0 k : = 0 repeat α k : = p k T A p k r k T r k x k + 1 : = x k + α k p k r k + 1 : = r k − α k A p k if r k + 1 is sufficiently small, then exit loop β k : = r k T r k r k + 1 T r k + 1 p k + 1 : = r k + 1 + β k p k k : = k + 1 end repeat return x k + 1 as the result
部分值推導如下：
預處理(preconditioning)是一種將問題調整爲更適合數值求解方法的形式的變換，使得條件數變小。預處理器的選擇參考preconditioner wiki ，一般選用雅克比預處理器，最簡單即對角線。
參考：wiki
上面的優化最終都會化簡爲A x = b Ax=b A x = b n × n n\times n n × n
值得注意的是，迭代優化求解效果往往與初值有很大的關係，初值不好容易收斂到局部最小。
我們一般假設所求的狀態x x x J T Σ − 1 J Δ x = − J T Σ − 1 b (34)
{\rm J^T\Sigma^{-1}J}\Delta x=-{\rm J^T\Sigma^{-1}}b \tag{34}
J T Σ − 1 J Δ x = − J T Σ − 1 b ( 3 4 )
下面就以式（34）爲例進行求解。假設有3個相機4個觀測的點，將相機和點分開則Jacobi矩陣爲：J = [ A ∣ B ] = [ A 1 B 1 A 2 B 2 A 3 B 3 ] (35)
J=[A|B]=\left[ \begin{array}{ccc|c}
A_1&&&B_1\\
&A_2&&B_2\\
&&A_3&B_3
\end{array}\right] \tag{35}
J = [ A ∣ B ] = ⎣ ⎡ A 1 A 2 A 3 B 1 B 2 B 3 ⎦ ⎤ ( 3 5 ) Δ x = [ Δ x c Δ x p ] b = [ A T Σ − 1 b B T Σ − 1 b ] = [ b A b B ] (36)
\Delta x=
\left[ \begin{array} {cc}
\Delta x_c \\
\hline
\Delta x_p
\end{array} \right]
\quad b=
\left[ \begin{array} {cc}
A^T\Sigma^{-1} b \\
\hline
B^T\Sigma^{-1} b
\end{array} \right]=\begin{bmatrix}
b_A \\ b_B
\end{bmatrix}
\tag{36}
Δ x = [ Δ x c Δ x p ] b = [ A T Σ − 1 b B T Σ − 1 b ] = [ b A b B ] ( 3 6 )
H = J T Σ − 1 J = [ A T Σ − 1 A A T Σ − 1 B B T Σ − 1 A B T Σ − 1 B ] = [ U W W T V ] (37)
H=J^T\Sigma^{-1}J=
\left[\begin{array}{c|c}
A^T\Sigma^{-1}A & A^T\Sigma^{-1}B \\
\hline
B^T\Sigma^{-1}A &B^T\Sigma^{-1}B
\end{array}\right]
=\begin{bmatrix}
U&W\\
W^T&V
\end{bmatrix}
\tag{37}
H = J T Σ − 1 J = [ A T Σ − 1 A B T Σ − 1 A A T Σ − 1 B B T Σ − 1 B ] = [ U W T W V ] ( 3 7 )
這樣公式可以表示爲
[ U W W T V ] [ Δ x c Δ x p ] = [ b A b B ] (38)
\begin{bmatrix}
U&W\\
W^T&V
\end{bmatrix}
\left[ \begin{array} {cc}
\Delta x_c \\
\Delta x_p
\end{array} \right]=\begin{bmatrix}
b_A \\b_B
\end{bmatrix} \tag{38}
[ U W T W V ] [ Δ x c Δ x p ] = [ b A b B ] ( 3 8 )
根據公式（18）-（21），計算V V V ( U − W V − 1 W T ) Δ x c = b A − W V − 1 b B (39)
(U-WV^{-1}W^T)\Delta x_c=b_A-WV^{-1}b_B \tag{39}
( U − W V − 1 W T ) Δ x c = b A − W V − 1 b B ( 3 9 ) Δ x c \Delta x_c Δ x c Δ x p \Delta x_p Δ x p V Δ x p = b B − W T Δ x c (40)
V\Delta x_p=b_B-W^T\Delta x_c \tag{40}
V Δ x p = b B − W T Δ x c ( 4 0 ) [ U − W V − 1 W T 0 W T V ] [ Δ x c Δ x p ] = [ b A − W V − 1 b B b B ] (41)
\begin{bmatrix}
U-WV^{-1}W^T&0\\
W^T&V
\end{bmatrix}
\left[ \begin{array} {cc}
\Delta x_c \\
\Delta x_p
\end{array} \right]=\begin{bmatrix}
b_A-WV^{-1}b_B \\b_B
\end{bmatrix} \tag{41}
[ U − W V − 1 W T W T 0 V ] [ Δ x c Δ x p ] = [ b A − W V − 1 b B b B ] ( 4 1 )
根據公式（34）可知，狀態增量Δ \Delta Δ b b b Δ x = − ( J T Σ − 1 J ) − 1 J T Σ − 1 b (42)
\Delta x=-({\rm J^T\Sigma^{-1}J})^{-1}{\rm J^T\Sigma^{-1}}b \tag{42}
Δ x = − ( J T Σ − 1 J ) − 1 J T Σ − 1 b ( 4 2 ) Σ p = A Σ k A T = ( J T Σ − 1 J ) + J T Σ − 1 Σ ( ( J T Σ − 1 J ) + J T Σ − 1 ) T = ( J T Σ − 1 J ) + (43)
\begin{aligned}
\Sigma_{p} =A\Sigma_k A^T &=({\rm J^T\Sigma^{-1}J})^{ + }{\rm J^T\Sigma^{-1}} \ \Sigma \ \left(({\rm J^T\Sigma^{-1}J})^{+}{\rm J^T\Sigma^{-1}}\right)^T \\
&=({\rm J^T\Sigma^{-1}J})^{+}
\end{aligned} \tag{43}
Σ p = A Σ k A T = ( J T Σ − 1 J ) + J T Σ − 1 Σ ( ( J T Σ − 1 J ) + J T Σ − 1 ) T = ( J T Σ − 1 J ) + ( 4 3 ) J T Σ X − 1 J = [ U W W ⊤ V ] = [ I Y 0 I ] [ U − W V − 1 W T 0 0 V ] [ I 0 Y T I ] (44)
\mathrm { J } ^ { \rm T } \Sigma _ { \mathbf { X } } ^ { - 1 } \mathrm { J } = \left[ \begin{array} { c c } { U } & { W } \\ { W ^ { \top } } & { V } \end{array} \right]=\left[ \begin{array} { c c } { \mathrm { I } } & { \mathrm { Y } } \\ { 0 } & { \mathrm { I } } \end{array} \right] \left[ \begin{array} { c c } { \mathrm { U } - \mathrm { WV } ^ { - 1 } \mathrm { W } ^ { \mathrm { T } } } & { 0 } \\ { 0 } & { \mathrm { V } } \end{array} \right] \left[ \begin{array} { c c } { \mathrm { I } } & { 0 } \\ { \mathrm { Y } ^ { \mathrm { T } } } & { \mathrm { I } } \end{array} \right] \tag{44}
J T Σ X − 1 J = [ U W ⊤ W V ] = [ I 0 Y I ] [ U − W V − 1 W T 0 0 V ] [ I Y T 0 I ] ( 4 4 ) Y = W V − 1 Y=WV^{-1} Y = W V − 1 G G G ( G H G T ) + = G − T H + G − 1 (45)
\left( \mathrm { GHG } ^ { \mathrm { T } } \right) ^ { + } = \mathrm { G } ^ { - \mathrm { T } } \mathrm { H } ^ { + } \mathrm { G } ^ { - 1 } \tag{45}
( G H G T ) + = G − T H + G − 1 ( 4 5 ) Σ P = ( J T Σ X − 1 J ) + = [ X − X Y − Y T X Y T X Y + V − 1 ] (46)
\Sigma _ { \mathbf { P } } = \left( \mathbf { J } ^ {\rm T } \Sigma _ { \mathbf { X } } ^ { - 1 } \mathbf { J } \right) ^ { + } = \left[ \begin{array} { c c } { X } & { - X Y } \\ { - Y ^ { \rm T } X } & { Y ^ { \rm T } X Y + V ^ { - 1 } } \end{array} \right] \tag{46}
Σ P = ( J T Σ X − 1 J ) + = [ X − Y T X − X Y Y T X Y + V − 1 ] ( 4 6 ) X = ( U − W V − 1 W T ) + \rm X=(U-WV^{-1}W^T)^{+} X = ( U − W V − 1 W T ) +
通常的算法框架如下圖：
第一種稀疏形式是由於，一些點並不是每一幀都可以被觀測到的。就有一些觀測x i j x_{ij} x i j A i j , B i j , Σ x i j − 1 , W i j , Y i j A_{ij}, B_{ij}, \Sigma^{-1}_{x_{ij}}, W_{ij}, Y_{ij} A i j , B i j , Σ x i j − 1 , W i j , Y i j
第二種是帶狀結構。
S j k = − ∑ i W i j V i − 1 W i k T if j ≠ k (47)
\mathrm { S } _ { j k } = - \sum _ { i } \mathrm { W } _ { i j } {\rm V}^{\rm -1}_i\mathrm { W } _ { i k } ^ { \mathrm { T } } \text { if } j \neq k \tag{47}
S j k = − i ∑ W i j V i − 1 W i k T if j = k ( 4 7 )
如果點是被連續跟蹤幾幀的情況下，則（47）式是帶狀結構的，因爲只有W i j W_{ij} W i j W i k W_{ik} W i k i i i j j j k k k S j k S_{jk} S j k
如果在重定位和閉環過程中，這種連續就被破壞了，不具有帶狀結構。
對於這種結構可以使用因式分解來求解，如LDLT分解，Cholesky分解，我們安裝的suitSparse或者Chlmod等庫，是加速求解的。
此外還可以使用這種方法求解F矩陣和H矩陣，就看MVG就行了。
這種也叫作是M估計，有兩種方式：1. 魯棒核函數作用在殘差平方上；2. 魯棒核函數作用在e T W − 1 e \sqrt{e^TW^{-1}e} e T W − 1 e
狀態估計第5章講的是方法2，我們這裏使用第一種方式（Triggs Correction）爲例：min x 1 2 ∑ k ρ ( ∥ f k ( x ) ∥ 2 ) (48)
\min _{\mathbf{x}} \frac{1}{2} \sum_{k} \rho\left(\left\|f_{k}(\mathbf{x})\right\|^{2}\right) \tag{48}
x min 2 1 k ∑ ρ ( ∥ f k ( x ) ∥ 2 ) ( 4 8 ) s k = ∥ f k ( x ) ∥ 2 s_{k}=\left\|f_{k}(\mathbf{x})\right\|^{2} s k = ∥ f k ( x ) ∥ 2 1 2 ρ ( s ) = 1 2 ( c o n s t + ρ ′ Δ s + 1 2 ρ ′ ′ Δ s 2 ) (49)
\frac{1}{2} \rho(s)=\frac{1}{2}\left(\mathrm{const}+\rho^{\prime} \Delta s+\frac{1}{2} \rho^{\prime \prime} \Delta s^{2} \right) \tag{49}
2 1 ρ ( s ) = 2 1 ( c o n s t + ρ ′ Δ s + 2 1 ρ ′ ′ Δ s 2 ) ( 4 9 )
Δ s k = ∥ f k ( x + Δ x ) ∥ 2 − ∥ f k ( x ) ∥ 2 ≈ ∥ f k + J k Δ x ∥ 2 − ∥ f k ( x ) ∥ 2 = 2 f k ⊤ J k Δ x + ( Δ x ) ⊤ J k ⊤ J k Δ x (50)
\begin{aligned} \Delta s_{k} &=\left\|f_{k}(\mathbf{x}+\Delta \mathbf{x})\right\|^{2}-\left\|f_{k}(\mathbf{x})\right\|^{2} \\ & \approx\left\|f_{k}+\mathbf{J}_{k} \Delta \mathbf{x}\right\|^{2}-\left\|f_{k}(\mathbf{x})\right\|^{2} \\ &=2 f_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}+(\Delta \mathbf{x})^{\top} \mathbf{J}_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x} \end{aligned} \tag{50}
Δ s k = ∥ f k ( x + Δ x ) ∥ 2 − ∥ f k ( x ) ∥ 2 ≈ ∥ f k + J k Δ x ∥ 2 − ∥ f k ( x ) ∥ 2 = 2 f k ⊤ J k Δ x + ( Δ x ) ⊤ J k ⊤ J k Δ x ( 5 0 )
將公式（50）帶入到公式（49），並且忽略三次以上的項：1 2 ρ ( s ) ≈ 1 2 ( ρ ′ [ 2 f k ⊤ J k Δ x + ( Δ x ) ⊤ J k ⊤ J k Δ x ] + 1 2 ρ ′ ′ [ 2 f k ⊤ J k Δ x + ( Δ x ) ⊤ J k ⊤ J k Δ x ] 2 + const ) ≈ ρ ′ f k ⊤ J k Δ x + 1 2 ρ ′ ( Δ x ) ⊤ J k ⊤ J k Δ x + ρ ′ ′ ( Δ x ) ⊤ J k ⊤ f k f k ⊤ J k Δ x + c o n s t = ρ ′ f k ⊤ J k Δ x + 1 2 ( Δ x ) ⊤ J k ⊤ ( ρ ′ I + 2 ρ ′ ′ f k f k ⊤ ) J k Δ x + c o n s t (51)
\begin{aligned} \frac{1}{2} \rho(s) \approx & \frac{1}{2}\left(\rho^{\prime}\left[2 f_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}+(\Delta \mathbf{x})^{\top} \mathbf{J}_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}\right]\right.\\ &+\frac{1}{2} \rho^{\prime \prime}\left[2 f_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}+(\Delta \mathbf{x})^{\top} \mathbf{J}_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}\right]^{2}+\text { const } ) \\ \approx & \rho^{\prime} f_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}+\frac{1}{2} \rho^{\prime}(\Delta \mathbf{x})^{\top} \mathbf{J}_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}+\rho^{\prime \prime}(\Delta \mathbf{x})^{\top} \mathbf{J}_{k}^{\top} f_{k} f_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}+c o n s t \\=& \rho^{\prime} f_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}+\frac{1}{2}(\Delta \mathbf{x})^{\top} \mathbf{J}_{k}^{\top}\left(\rho^{\prime} I+2 \rho^{\prime \prime} f_{k} f_{k}^{\top}\right) \mathbf{J}_{k} \Delta \mathbf{x}+c o n s t \end{aligned} \tag{51}
2 1 ρ ( s ) ≈ ≈ = 2 1 ( ρ ′ [ 2 f k ⊤ J k Δ x + ( Δ x ) ⊤ J k ⊤ J k Δ x ] + 2 1 ρ ′ ′ [ 2 f k ⊤ J k Δ x + ( Δ x ) ⊤ J k ⊤ J k Δ x ] 2 + const ) ρ ′ f k ⊤ J k Δ x + 2 1 ρ ′ ( Δ x ) ⊤ J k ⊤ J k Δ x + ρ ′ ′ ( Δ x ) ⊤ J k ⊤ f k f k ⊤ J k Δ x + c o n s t ρ ′ f k ⊤ J k Δ x + 2 1 ( Δ x ) ⊤ J k ⊤ ( ρ ′ I + 2 ρ ′ ′ f k f k ⊤ ) J k Δ x + c o n s t ( 5 1 ) ∑ k J k ⊤ ( ρ ′ I + 2 ρ ′ ′ f k f k ⊤ ) J k Δ x = − ∑ k ρ ′ f k ⊤ J k ∑ k J k ⊤ W J k Δ x = − ∑ k n ρ ′ f k ⊤ J k (52)
\sum_{k} \mathbf{J}_{k}^{\top}\left(\rho^{\prime} I+2 \rho^{\prime \prime} f_{k} f_{k}^{\top}\right) \mathbf{J}_{k} \Delta \mathbf{x}=-\sum_{k} \rho^{\prime} f_{k}^{\top} \mathbf{J}_{k}
\\
\sum_{k} \mathbf{J}_{k}^{\top} W \mathbf{J}_{k} \Delta \mathbf{x}=-\sum_{k}^{n} \rho^{\prime} f_{k}^{\top} \mathbf{J}_{k} \tag{52}
k ∑ J k ⊤ ( ρ ′ I + 2 ρ ′ ′ f k f k ⊤ ) J k Δ x = − k ∑ ρ ′ f k ⊤ J k k ∑ J k ⊤ W J k Δ x = − k ∑ n ρ ′ f k ⊤ J k ( 5 2 ) ρ ( s ) = c 2 log ( 1 + s c 2 ) (53)
\rho(s)=c^{2} \log \left(1+\frac{s}{c^{2}}\right) \tag{53}
ρ ( s ) = c 2 log ( 1 + c 2 s ) ( 5 3 ) ρ ′ ( s ) = 1 1 + s c 2 , ρ ′ ′ ( s ) = − 1 c 2 ( ρ ′ ( s ) ) 2 (54)
\rho^{\prime}(s)=\frac{1}{1+\frac{s}{c^{2}}}, \qquad \rho^{\prime \prime}(s)=-\frac{1}{c^{2}}\left(\rho^{\prime}(s)\right)^{2} \tag{54}
ρ ′ ( s ) = 1 + c 2 s 1 , ρ ′ ′ ( s ) = − c 2 1 ( ρ ′ ( s ) ) 2 ( 5 4 )
如果殘差符合正態分佈，Huber c = 1.345，Cauchy c = 2.3849.
如果殘差不是正態分佈，則使用median absolute residual估計殘差方差，然後對殘差進行歸一化
σ = 1.482 ⋅ med ( med ( r ) − r i ) (55)
\sigma=1.482 \cdot \operatorname{med}\left(\operatorname{med}(r)-r_{i}\right) \tag{55}
σ = 1 . 4 8 2 ⋅ m e d ( m e d ( r ) − r i ) ( 5 5 )
估計誤差尺度的方法，當作標準差，對殘差歸一化用（參考rpg_vikit）：
estimates scale by fitting a t-distribution to the data with the given degrees of freedom
estimates scale by computing the median absolute deviation
estimates scale by computing the standard deviation
float NormalDistributionScaleEstimator::
compute(std::vector<float>& errors) const
{
const float mean = std::accumulate(errors.begin(), errors.end(), 0)/errors.size();
float var = 0.0;
std::for_each(errors.begin(), errors.end(), [&](const float d) {
var += (d - mean) * (d - mean);
});
return std::sqrt(var); // return standard deviation
}
// 因爲MAD是比樣本方差或標準差更穩健的量表估計，所以對於沒有均值或方差的分佈，它更好
//* 對於正態分佈來說係數是1.4826來估計標準差, 參考https://en.wikipedia.org/wiki/Median_absolute_deviation
const float MADScaleEstimator::NORMALIZER = 1.48f; // 1 / 0.6745
float MADScaleEstimator::
compute(std::vector<float>& errors) const
{
// error must be in absolute values!
return NORMALIZER * vk::getMedian(errors);
}
關於魯棒核函數的代碼也可以參考 rpg_vikit
此外就是使用isam和isam2這種增量式的求解公式（34）的問題，由於其利用稀疏性速度較快，需要排序來保證稀疏性，對應之前講的排列矩陣。而滑動窗口式的則由於邊緣化掉一些點，導致矩陣變得稠密不在具有稀疏形式，求解較慢。DSO是直接把這些fill-in置爲0.
在SfM中還使用迭代Schur和Precondition Conjugate Gradient共軛梯度法，這種算法的思想是在每個子空間內分別求解。在SLAM中用的不多，維度沒有SfM大，只要order做的好不需要共軛梯度。
滑窗的是把舊幀邊緣化掉，以一定的概率加到優化中，先驗是可能正確的。當前的最小二乘項和先驗項互相約束，得到最優解。
co-visibility的方式是把大的窗口外的3D點固定，相當於fix，這樣後面的狀態變得可觀了。相當於假設先驗是一定正確的。
從概率方式理解：p ( a , b ) = p ( a ∣ b ) p ( b )
p(a,b)=p(a|b)p(b)
p ( a , b ) = p ( a ∣ b ) p ( b ) p ( b ) p(b) p ( b ) p ( b ) = 1 p(b)=1 p ( b ) = 1
統計了直接法中的亮度誤差是符合t分佈（student分佈）的，而不是像特徵點那樣符合高斯分佈的。
特徵點符合高斯分佈是和提取中的過程有關，使用高斯核來計算，在附近取一個圓形區域。需要詳細研究下。
![[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-gDC16Xoy-1582200908839)(img/最小二乘優化/1547532395665.png)]](https://pic1.xuehuaimg.com/proxy/csdn/https://img-blog.csdnimg.cn/2020022022474640.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L09LYXN5,size_16,color_FFFFFF,t_70)
![[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-6XMEX98a-1582200908843)(img/最小二乘優化/1547733246804.png)]](https://pic1.xuehuaimg.com/proxy/csdn/https://img-blog.csdnimg.cn/20200220225001623.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L09LYXN5,size_16,color_FFFFFF,t_70)
