一.歷史由來

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等等。

[Triggs00] Bundle Adjustment - A Modern Synthesis, Bill Triggs, et al. [Hartley00] Multiple View Geometry in Computer Vision, Richard Hartley, Andrew Zisserman [Johannsson13] H. Johannsson, M. Kaess, M. Fallon, and J. J. Leonard, “Temporally scalable visual slam using a reduced pose graph,” in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), 2013, pp. 54–61. [Grisetti10] G. Grisetti, R. Kummerle, C. Stachniss, and W. Burgard, “A tutorial on graph-based SLAM,” IEEE Transactions on Intelligent Transportation Systems Magazine, vol. 2, pp. 32–43, 2010. [Agarwal14] A Survey of Geodetic Approaches to Mapping and the Relationship to Graph-based SLAM

二.Bundle Adjustment到底是什么？ http://blog.csdn.net/OptSolution/article/details/64442962

譯為光束法平差，或者束調整、捆集調整。

所謂bundle，來源于bundle of light，其本意就是指的光束，這些光束指的是三維空間中的點投影到像平面上的光束，而重投影誤差正是利用這些光束來構建的，因此稱為光束法，強調光束也正是描述其優化模型是如何建立的。剩下的就是平差，那什么是平差呢？

測量平差：由于測量儀器的精度不完善和人為因素及外界條件的影響，測量誤差總是不可避免的。為了提高成果的質量，處理好這些測量中存在的誤差問題，觀測值的個數往往要多于確定未知量所必須觀測的個數，也就是要進行多余觀測。有了多余觀測，勢必在觀測結果之間產生矛盾，測量平差的目的就在于消除這些矛盾而求得觀測量的最可靠結果并評定測量成果的精度。測量平差采用的原理就是“最小二乘法”。

[1]BA模型：

BA的本質是一個優化模型，其目的是最小化重投影誤差.

看！這些五顏六色的線就是我們講的光束！那現在就該說下什么叫重投影誤差了，重投影也就是指的第二次投影：

其實第一次投影指的就是相機在拍照的時候三維空間點投影到圖像上 然后我們利用這些圖像對一些特征點進行三角定位（triangulation），利用幾何信息（對極幾何） 構建三角形來確定三維空間點的位置 最后利用我們計算得到的三維點的坐標（注意不是真實的）和我們計算得到的相機位姿（當然也不是真實的）進行第二次投影，也就是重投影。 重投影誤差：指的真實三維空間點在圖像平面上的投影（也就是圖像上的像素點）和重投影（其實是用我們的計算值得到的虛擬的像素點）的差值， 因為種種原因計算得到的值和實際情況不會完全相符，也就是這個差值不可能恰好為0，此時也就需要將這些差值的和最小化獲取最優的相機位姿參數及三維空間點的坐標。

[2]BA的數學模型

對BA有點了解的同學可能知道BA是一個圖優化模型，那首先肯定要構造一個圖模型了。既然是圖模型那自然就有節點和邊了，

這個圖模型的節點由相機和三維空間點構成構成，如果點投影到相機的圖像上則將這兩個節點連接起來。

下圖所示：

[3]計算---非線性優化

可以使用各種優化算法來進行計算，BA現在基本都是利用LM（Levenberg-Marquardt）算法并在此基礎上利用BA模型的稀疏性質來進行計算的，

LM算法是最速下降法（梯度下降法）和Gauss-Newton的結合體。

（1）最速下降法

如果對梯度比較熟悉的話，那應該知道梯度方向是函數上升最快的方向，而此時我們需要解決的問題是讓函數最小化。

你應該想到了，那就順著梯度的負方向去迭代尋找使函數最小的變量值。梯度下降法就是用的這種思想，用數學表達：

xk=xk−1−λ∇f(xk−1)

其中λ為步長。最速下降法保證了每次迭代函數都是下降的，在初始點離最優點很遠的時候剛開始下降的速度非常快，

但是最速下降法的迭代方向是折線形的導致了收斂非常非常的慢。

（2）Newton型方法

現在先回顧一下中學數學，給定一個開口向上的一元二次函數，如何知道該函數何處最小？這個應該很容易就可以答上來了，對該函數求導，導數為0處就是函數最小處。

Newton型方法也就是這種思想，首先將函數利用泰勒展開到二次項：

（3）Gauss-Newton方法

既然Newton型方法計算Hessian矩陣太困難了，那有沒有什么方法可以不計算Hessian矩陣呢？將泰勒展開式的二次項也去掉好像就可以避免求Hessian矩陣了吧，就像這樣：

（4）LM（Levenberg-Marquadt）方法

其實LM算法的具體形式就筆者看到的就有很多種，但是本質都是通過參數λ在最速下降法和Gauss-Newton法之間切換。這里選用的是維基百科上的形式。

LM算法就由此保證了每次迭代都是下降的，并且可以快速收斂。

[4]解方程

LM算法主體就是一個方程的求解，也是其計算量最大的部分。當其近似于最速下降法的時候沒有什么好討論的，但是當其近似于Gauss-Newton法的時候，

這個最小二乘解的問題就該好好討論一下了。以下的討論就利用Gauss-Newton的形式來求解。

(1)稠密矩陣的最小二乘解

(2)稀疏矩陣的Cholesky分解

稀疏矩陣的話利用其稀疏的性質可以大幅減少計算量，對于稀疏矩陣的Cholesky分解就是這樣。其分解形式為一個上三角矩陣的轉置乘上自身：

回到Gauss-Newton最后的超定參數方程吧。既然Jacobi矩陣可以分塊那我們就先分塊，分塊可以有效降低需要計算的矩陣的維度并以此減少計算量。

補充：

總結

以上是生活随笔為你收集整理的Bundle Adjustment---即最小化重投影误差（高翔slam---第七讲）的全部內容，希望文章能夠幫你解決所遇到的問題。

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