一.歷史由來(lái)

Adjustment computation最早是由geodesy的人搞出來(lái)的。19世紀(jì)中期的時(shí)候，geodetics的學(xué)者就開(kāi)始研究large scale triangulations（大型三角剖分）了。20世紀(jì)中期，隨著camera和computer的出現(xiàn)，photogrammetry(照相測(cè)量法)也開(kāi)始研究adjustment computation，所以他們給起了個(gè)名字叫bundle adjustment。21世紀(jì)前后，robotics領(lǐng)域開(kāi)始興起SLAM，最早用的recursive bayesian filter（遞歸貝葉斯濾波），后來(lái)把問(wèn)題搞成個(gè)graph然后用least squares方法解。

這些東西歸根結(jié)底就是Gauss大神“發(fā)明”的least squares method（最小二乘法）。當(dāng)年天文學(xué)家Piazzi整天閑得沒(méi)事看星星，在1801年1月1號(hào)早上發(fā)現(xiàn)了一個(gè)從來(lái)沒(méi)觀測(cè)到的星星，再接下來(lái)的42天里做了19次觀測(cè)之后這個(gè)星星就消失了。當(dāng)時(shí)的天文學(xué)家為了確定這玩意到底是什么絞盡了腦汁，這時(shí)候Gauss出現(xiàn)了，（最初）只用了3個(gè)觀察數(shù)據(jù)，就用least squares算出了這個(gè)小行星的軌道，接下來(lái)天文學(xué)家根據(jù)Gauss的預(yù)測(cè)，也重新發(fā)現(xiàn)了這個(gè)小行星（雖然有小小的偏差），并將其命名為Ceres，也就是谷神星。Google的ceres-solver就是根據(jù)這個(gè)來(lái)命名的。[ref: How Gauss Determined the Orbit of Ceres]

Bundle adjustment優(yōu)化的是sum of reprojection error，這是一個(gè)（geometric distance）幾何距離[為什么要minimize geometric distance可以參考[Hartley00]]，可以轉(zhuǎn)換成一個(gè)least squares problem， 如果nosie是gaussian的話，那就是一個(gè)最大似然估計(jì)（maximum likelihood estimator），是這種情況下所能得到的最優(yōu)解了。 這個(gè)reprojection error的公式是非線性的，所以這個(gè)least squares problem得用迭代法來(lái)求解：般都是用Gauss-Newton 法或者LM算法迭代求解。bundle adjustmen由于是特定的形式，所以可以化成sparse matrix 的形式，這樣計(jì)算量大大減小了。不論GN,LM,中間都要解一個(gè)Ax=b形式的linear system，一般情況下算法的效率就取決于解這個(gè)linear system的效率。所以說(shuō)到底這些nonlinear least squares problem最后也就是解一個(gè)linear system。這個(gè)linear system你可以直接解，也可以用QR分解，喬姆斯基分解 ，或者奇異值分解法求解來(lái)解。

現(xiàn)實(shí)中，并不是所有觀測(cè)過(guò)程中的噪聲都服從 gaussian noise的（或者可以說(shuō)幾乎沒(méi)有），遇到有outlier的情況，這些方法非常容易掛掉，這時(shí)候就得用到robust statistics里面的robust cost(*cost也可以叫做loss, 統(tǒng)計(jì)學(xué)那邊喜歡叫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

譯為光束法平差，或者束調(diào)整、捆集調(diào)整。

所謂bundle，來(lái)源于bundle of light，其本意就是指的光束，這些光束指的是三維空間中的點(diǎn)投影到像平面上的光束，而重投影誤差正是利用這些光束來(lái)構(gòu)建的，因此稱(chēng)為光束法，強(qiáng)調(diào)光束也正是描述其優(yōu)化模型是如何建立的。剩下的就是平差，那什么是平差呢？

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

[1]BA模型：

BA的本質(zhì)是一個(gè)優(yōu)化模型，其目的是最小化重投影誤差.

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

其實(shí)第一次投影指的就是相機(jī)在拍照的時(shí)候三維空間點(diǎn)投影到圖像上 然后我們利用這些圖像對(duì)一些特征點(diǎn)進(jìn)行三角定位（triangulation），利用幾何信息（對(duì)極幾何） 構(gòu)建三角形來(lái)確定三維空間點(diǎn)的位置 最后利用我們計(jì)算得到的三維點(diǎn)的坐標(biāo)（注意不是真實(shí)的）和我們計(jì)算得到的相機(jī)位姿（當(dāng)然也不是真實(shí)的）進(jìn)行第二次投影，也就是重投影。 重投影誤差：指的真實(shí)三維空間點(diǎn)在圖像平面上的投影（也就是圖像上的像素點(diǎn)）和重投影（其實(shí)是用我們的計(jì)算值得到的虛擬的像素點(diǎn)）的差值， 因?yàn)榉N種原因計(jì)算得到的值和實(shí)際情況不會(huì)完全相符，也就是這個(gè)差值不可能恰好為0，此時(shí)也就需要將這些差值的和最小化獲取最優(yōu)的相機(jī)位姿參數(shù)及三維空間點(diǎn)的坐標(biāo)。

[2]BA的數(shù)學(xué)模型

對(duì)BA有點(diǎn)了解的同學(xué)可能知道BA是一個(gè)圖優(yōu)化模型，那首先肯定要構(gòu)造一個(gè)圖模型了。既然是圖模型那自然就有節(jié)點(diǎn)和邊了，

這個(gè)圖模型的節(jié)點(diǎn)由相機(jī)和三維空間點(diǎn)構(gòu)成構(gòu)成，如果點(diǎn)投影到相機(jī)的圖像上則將這兩個(gè)節(jié)點(diǎn)連接起來(lái)。

下圖所示：

[3]計(jì)算---非線性?xún)?yōu)化

可以使用各種優(yōu)化算法來(lái)進(jìn)行計(jì)算，BA現(xiàn)在基本都是利用LM（Levenberg-Marquardt）算法并在此基礎(chǔ)上利用BA模型的稀疏性質(zhì)來(lái)進(jìn)行計(jì)算的，

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

（1）最速下降法

如果對(duì)梯度比較熟悉的話，那應(yīng)該知道梯度方向是函數(shù)上升最快的方向，而此時(shí)我們需要解決的問(wèn)題是讓函數(shù)最小化。

你應(yīng)該想到了，那就順著梯度的負(fù)方向去迭代尋找使函數(shù)最小的變量值。梯度下降法就是用的這種思想，用數(shù)學(xué)表達(dá)：

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

其中λ為步長(zhǎng)。最速下降法保證了每次迭代函數(shù)都是下降的，在初始點(diǎn)離最優(yōu)點(diǎn)很遠(yuǎn)的時(shí)候剛開(kāi)始下降的速度非常快，

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

（2）Newton型方法

現(xiàn)在先回顧一下中學(xué)數(shù)學(xué)，給定一個(gè)開(kāi)口向上的一元二次函數(shù)，如何知道該函數(shù)何處最小？這個(gè)應(yīng)該很容易就可以答上來(lái)了，對(duì)該函數(shù)求導(dǎo)，導(dǎo)數(shù)為0處就是函數(shù)最小處。

Newton型方法也就是這種思想，首先將函數(shù)利用泰勒展開(kāi)到二次項(xiàng)：

（3）Gauss-Newton方法

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

（4）LM（Levenberg-Marquadt）方法

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

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

[4]解方程

LM算法主體就是一個(gè)方程的求解，也是其計(jì)算量最大的部分。當(dāng)其近似于最速下降法的時(shí)候沒(méi)有什么好討論的，但是當(dāng)其近似于Gauss-Newton法的時(shí)候，

這個(gè)最小二乘解的問(wèn)題就該好好討論一下了。以下的討論就利用Gauss-Newton的形式來(lái)求解。

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

(2)稀疏矩陣的Cholesky分解

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

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

補(bǔ)充：

總結(jié)

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

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。