在看高翔博士的《視覺slam十四講》，把看到的BA相關的東西記錄一下，方便自己之后復習，有理解不對的地方，還請指正！

一、非線性最小二乘

1.1 總體思路， 一階法和二階法

最小二乘問題 ，我們稱F(x)是代價函數， f(x)是誤差函數；

?直接求再比較各個極值點/鞍點的大小一般是比較難的。因此， 一般采取迭代的方法： 即從某個初值x0開始，每一次尋找一個可以使F(x)取得極小值的增量delta_x， 當增量delta_x比較小的時候，我們就認為認為達到了極值。

一階法尋找delta_x： 將F(x)做一階泰勒展開：，并取 ；這樣一定能夠保證代價函數值下降，一階法也叫最速下降法；

?二階法尋找delta_x： 將F(x)做二階泰勒展開： 且令 ，可得, 求解這個線性方程，就可以得到每一步的delta_x， 二階法也叫做牛頓法； H矩陣不好求，所以經常用高斯牛頓法等近似方法；

1.2 高斯牛頓法

對誤差f(x)做泰勒展開：, 每一次迭代時， 優化問題變為：

此時， 求增量delta_x，可以 ??直接令 ，得到 ，求解這個線性方程，就可以得到每一步的delta_x。這里可以認為， 高斯牛頓法用誤差函數的Jacobian相乘，近似了牛頓法中的代價函數的Hessian

?高斯法的不一定收斂，主要原因是：1） 不一定有逆矩陣， delta_x不太穩定；2）求出的delta_x太大， 導致一階泰勒近似失效；

1.3 LM法（Levenberg-Marquart）

LM法在一定程度的緩解了高斯牛頓法的問題，能夠提供更加穩定和準確的增量delta_x：1）給增量delta_x增加一個信賴區間，變成一個帶約束的優化問題；2）定義一個指標 p 刻畫誤差函數一階近似的好壞程度， 并據此動態改變信賴區間的大小。LM具體算法是：

?? ? ?? a. 給定初始值x0, 初始優化半徑u;

??????? b. 對于每一次迭代， 在高斯牛頓法的基礎上增加信賴區間， 求解：

??????? c. 計算誤差函數一階近似的好壞程度：

?? ? ?? d. 如果p太小， 說明當前位置梯度太大，信賴區間需要保守一些，則設置： u = 0.5u

??????? e. 如果p太大， 說明當前位置梯度比較小， 信賴區間可以激進一些， 則設置： u = 2u

??????? f. 如果p大于某個閾值， 則可以認為近似可行， 可以進行下一次迭代: x_k+1 = x_k+delta_x

??????? g. 判斷算法是否收斂；如果沒有收斂， 則用 f 中計算得到的x_k+1作為初值， 返回b開始新一輪迭代

b中帶約束的優化問題可以用拉格朗日乘數法求解上述 (學過SVM的同學請舉手~~)：

其核心仍然是解線性方程：，當λ較小時，說明二階近似比較好，基本等效于高斯牛頓法，當λ比較大的時候，說明二階近似不好，基本等效于一階法（最速下降法）

二、特征點法的BA(Bundle Adjustment， 光束法平差)

2.1 誤差函數和代價函數

特征點法認為路標點在圖片中投影的像素坐標已知， 因此可以最小化重投影誤差，來求解相機位姿和路標點位置，具體是：

?定義自變量x包含了所有帶求解的多幀位姿和多個路標點 ，位姿T在前，路標點p在后；

定義第j個路標在第i幀的誤差函數是：; 其中代表重投影誤差，? 代表第i幀相機位姿；代表第j個路標的位置；代表第j個路標在第i幀中投影的像素坐標；

?代價函數可以定義為多個路標在多幀中的重投影誤差的平方:? 。我們要求解的最優化問題定義為：， 即求取一組位姿和路標點，使得代價函數最小；

我們一般用迭代法求解上述最優化問題，因此我們實際關心的是求得每一步迭代時的增量delta_x, 即：， 其中， 是位姿的增量， 是地圖點的增量；

2.2 獲得每次迭代增量delta_x的線性方程

我們對誤差函數進行一階近似：

?其中:? , 是每個誤差函數對于x的jacobian;

令 , 得到關于增量delta_x的線性方程：

取就是高斯法， 取就是LM法；

J_ij只與第i幀和第j個路標點有關， 只在ii,? ij,? ji, jj四塊處取值；累加而成的H矩陣因此具有如下的稀疏結構：

B塊只與相機位姿有關，是對角陣； C塊只與路標點有關，是對角陣； E塊非零的地方表示對應的相機位置有對對應路標點的觀測；

?2.3 邊緣化（Schur消元）

增量方程可利用H矩陣的特點快速求解：

對增量方程進行劃分：， 并消去地圖點增量：

, 整理得到：

?可以得到只有位姿的方程：，C是對角矩陣，C的逆比較好求，這個方程比較好解；

再解出路標增量：， 同樣利用了C的逆比較好求；

以上消元的方法，先固定了路標點增量，求出位姿增量，再求出路標點的增量，從概率論的角度說先求了固定路標點下的條件概率， 再求路標點的邊緣概率，因此稱為邊緣化；

SLAM中的marginalization 和 Schur complement_白巧克力亦唯心的博客-CSDN博客_schur消元

總結

以上是生活随笔為你收集整理的非线性最小二乘， BA(Bundle Adjustment)的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。