在SFM（structure from motion）的計算中BA（Bundle Adjustment）作爲最後一步優化具有很重要的作用，在近幾年興起的基於圖的SLAM（simultaneous localization and mapping）算法裏面使用了圖優化替代了原來的濾波器，這裏所謂的圖優化其實也是指BA。其實很多經典的文獻對於BA都有深深淺淺的介紹，如果想對BA的全過程做一個全面的更深層次的瞭解，推薦閱讀 Bundle Adjustment —A Modern Synthesis.
目錄
平差也就正好表述了爲什麼需要BA以及BA這個優化過程到底是怎麼進行的。
一. 歷史由來
Adjustment computation最早是由geodesy的人搞出來的。19世紀中期的時候，geodetics的學者就開始研究large scale triangulations（大型三角剖分）。20世紀中期，隨着camera和computer的出現，photogrammetry(照相測量法)也開始研究adjustment computation，所以他們給起了個名字叫bundle adjustment。21世紀前後，robotics領域開始興起SLAM，最早用的recursive bayesian filter（遞歸貝葉斯濾波），後來把問題搞成個graph然後用least squares方法解。
這些東西歸根結底就是Gauss大神“發明”的least squares method（最小二乘法）。當年天文學家Piazzi整天閒得沒事看星星，在1801年1月1號早上發現了一個從來沒觀測到的星星，再接下來的42天裏做了19次觀測之後這個星星就消失了。當時的天文學家爲了確定這玩意到底是什麼絞盡了腦汁，這時候Gauss出現了，（最初）只用了3個觀察數據，就用least squares算出了這個小行星的軌道，接下來天文學家根據Gauss的預測，也重新發現了這個小行星（雖然有小小的偏差），並將其命名爲Ceres，也就是穀神星。Google的ceres-solver就是根據這個來命名的。[ref: How Gauss Determined the Orbit of Ceres]
Bundle adjustment優化的是sum of reprojection error，即重投影誤差和，這是一個（geometric distance）幾何距離[爲什麼要minimize geometric distance可以參考[Hartley00]]，可以轉換成一個least squares problem， 如果nosie是gaussian的話，那就是一個最大似然估計（maximum likelihood estimator），是這種情況下所能得到的最優解了。 這個reprojection error的公式是非線性的，所以這個least squares problem得用迭代法來求解：一般都是用Gauss-Newton 法或者LM算法迭代求解。bundle adjustmen由於是特定的形式，所以可以化成sparse matrix 稀疏矩陣的形式，這樣計算量大大減小了。不論GN,LM,中間都要解一個Ax=b形式的linear system，一般情況下算法的效率就取決於解這個linear system的效率。所以說到底這些nonlinear least squares problem最後也就是解一個linear system。這個linear system你可以直接解，也可以用QR分解，喬姆斯基分解 ，或者奇異值分解法求解來解。
然而，現實中並不是所有觀測過程中的噪聲都服從 gaussian noise的（或者可以說幾乎沒有），遇到有outlier的情況，這些方法非常容易掛掉，這時候就得用到robust statistics裏面的robust cost(*cost也可以叫做loss, 統計學那邊喜歡叫risk) function了，比較常用的有huber, cauchy等等。
二. Bundle Adjustment到底是什麼？
博客https://blog.csdn.net/OptSolution/article/details/64442962 詳細介紹了BA和SLAM圖優化方法。
BA 譯爲光束法平差，或者束調整、捆集調整。
所謂bundle，來源於bundle of light，其本意就是指的光束，這些光束指的是三維空間中的點投影到像平面上的光束，而重投影誤差正是利用這些光束來構建的，因此稱爲光束法，強調光束也正是描述其優化模型是如何建立的。剩下的就是平差，那什麼是平差呢？
| 測量平差：由於測量儀器的精度不完善和人爲因素及外界條件的影響，測量誤差總是不可避免的。爲了提高成果的質量，處理好這些測量中存在的誤差問題，觀測值的個數往往要多於確定未知量所必須觀測的個數，也就是要進行多餘觀測。有了多餘觀測，勢必在觀測結果之間產生矛盾，測量平差的目的就在於消除這些矛盾而求得觀測量的最可靠結果並評定測量成果的精度。測量平差採用的原理就是“最小二乘法”。 |
平差也就正好表述了爲什麼需要BA以及BA這個優化過程到底是怎麼進行的。
2.1 BA模型：
BA的本質是一個優化模型，其目的是最小化重投影誤差.
看！這些五顏六色的線就是我們講的光束！那現在就該說下什麼叫重投影誤差了，重投影也就是指的第二次投影：
重投影誤差：指的真實三維空間點在圖像平面上的投影（也就是圖像上的像素點）和重投影（其實是用我們的計算值得到的虛擬的像素點）的差值， 因爲種種原因計算得到的值和實際情況不會完全相符，也就是這個差值不可能恰好爲0，此時也就需要將這些差值的和最小化獲取最優的相機位姿參數及三維空間點的座標。 |
2.2 BA的數學模型
對BA有點了解的同學可能知道BA是一個圖優化模型，那首先肯定要構造一個圖模型了。既然是圖模型那自然就有節點和邊了，
這個圖模型的節點由相機
下圖所示：
2.3 計算---非線性優化
可以使用各種優化算法來進行計算，BA現在基本都是利用LM（Levenberg-Marquardt）算法並在此基礎上利用BA模型的稀疏性質來進行計算的，
LM算法是最速下降法（梯度下降法）和Gauss-Newton的結合體。
（1）最速下降法
如果對梯度比較熟悉的話，那應該知道梯度方向是函數上升最快的方向，而此時我們需要解決的問題是讓函數最小化。
你應該想到了，那就順着梯度的負方向去迭代尋找使函數最小的變量值。梯度下降法就是用的這種思想，用數學表達：
其中λ爲步長。最速下降法保證了每次迭代函數都是下降的，在初始點離最優點很遠的時候剛開始下降的速度非常快，
但是最速下降法的迭代方向是折線形的導致了收斂非常非常的慢。
（2）Newton型方法
現在先回顧一下中學數學，給定一個開口向上的一元二次函數，如何知道該函數何處最小？這個應該很容易就可以答上來了，對該函數求導，導數爲0處就是函數最小處。
Newton型方法也就是這種思想，首先將函數利用泰勒展開到二次項：
（3）Gauss-Newton方法
既然Newton型方法計算Hessian矩陣太困難了，那有沒有什麼方法可以不計算Hessian矩陣呢？將泰勒展開式的二次項也去掉好像就可以避免求Hessian矩陣了吧，就像這樣：
（4）LM（Levenberg-Marquadt）方法
其實LM算法的具體形式就筆者看到的就有很多種，但是本質都是通過參數λ在最速下降法和Gauss-Newton法之間切換。這裏選用的是維基百科上的形式。
LM算法就由此保證了每次迭代都是下降的，並且可以快速收斂。
2.4 解方程
LM算法主體就是一個方程的求解，也是其計算量最大的部分。當其近似於最速下降法的時候沒有什麼好討論的，但是當其近似於Gauss-Newton法的時候，
這個最小二乘解的問題就該好好討論一下了。以下的討論就利用Gauss-Newton的形式來求解。
(1)稠密矩陣的最小二乘解
(2)稀疏矩陣的Cholesky分解
稀疏矩陣的話利用其稀疏的性質可以大幅減少計算量，對於稀疏矩陣的Cholesky分解就是這樣。其分解形式爲一個上三角矩陣的轉置乘上自身：
回到Gauss-Newton最後的超定參數方程吧。既然Jacobi矩陣可以分塊那我們就先分塊，分塊可以有效降低需要計算的矩陣的維度並以此減少計算量。
補充：
