在看高翔博士的《視覺(jué)slam十四講》，把看到的BA相關(guān)的東西記錄一下，方便自己之后復(fù)習(xí)，有理解不對(duì)的地方，還請(qǐng)指正！

一、非線性最小二乘

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

最小二乘問(wèn)題 ，我們稱(chēng)F(x)是代價(jià)函數(shù)， f(x)是誤差函數(shù)；

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

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

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

1.2 高斯牛頓法

對(duì)誤差f(x)做泰勒展開(kāi)：, 每一次迭代時(shí)， 優(yōu)化問(wèn)題變?yōu)?#xff1a;

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

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

1.3 LM法（Levenberg-Marquart）

LM法在一定程度的緩解了高斯牛頓法的問(wèn)題，能夠提供更加穩(wěn)定和準(zhǔn)確的增量delta_x：1）給增量delta_x增加一個(gè)信賴(lài)區(qū)間，變成一個(gè)帶約束的優(yōu)化問(wèn)題；2）定義一個(gè)指標(biāo) p 刻畫(huà)誤差函數(shù)一階近似的好壞程度， 并據(jù)此動(dòng)態(tài)改變信賴(lài)區(qū)間的大小。LM具體算法是：

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

??????? b. 對(duì)于每一次迭代， 在高斯牛頓法的基礎(chǔ)上增加信賴(lài)區(qū)間， 求解：

??????? c. 計(jì)算誤差函數(shù)一階近似的好壞程度：

?? ? ?? d. 如果p太小， 說(shuō)明當(dāng)前位置梯度太大，信賴(lài)區(qū)間需要保守一些，則設(shè)置： u = 0.5u

??????? e. 如果p太大， 說(shuō)明當(dāng)前位置梯度比較小， 信賴(lài)區(qū)間可以激進(jìn)一些， 則設(shè)置： u = 2u

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

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

b中帶約束的優(yōu)化問(wèn)題可以用拉格朗日乘數(shù)法求解上述 (學(xué)過(guò)SVM的同學(xué)請(qǐng)舉手~~)：

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

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

2.1 誤差函數(shù)和代價(jià)函數(shù)

特征點(diǎn)法認(rèn)為路標(biāo)點(diǎn)在圖片中投影的像素坐標(biāo)已知， 因此可以最小化重投影誤差，來(lái)求解相機(jī)位姿和路標(biāo)點(diǎn)位置，具體是：

?定義自變量x包含了所有帶求解的多幀位姿和多個(gè)路標(biāo)點(diǎn) ，位姿T在前，路標(biāo)點(diǎn)p在后；

定義第j個(gè)路標(biāo)在第i幀的誤差函數(shù)是：; 其中代表重投影誤差，? 代表第i幀相機(jī)位姿；代表第j個(gè)路標(biāo)的位置；代表第j個(gè)路標(biāo)在第i幀中投影的像素坐標(biāo)；

?代價(jià)函數(shù)可以定義為多個(gè)路標(biāo)在多幀中的重投影誤差的平方:? 。我們要求解的最優(yōu)化問(wèn)題定義為：， 即求取一組位姿和路標(biāo)點(diǎn)，使得代價(jià)函數(shù)最小；

我們一般用迭代法求解上述最優(yōu)化問(wèn)題，因此我們實(shí)際關(guān)心的是求得每一步迭代時(shí)的增量delta_x, 即：， 其中， 是位姿的增量， 是地圖點(diǎn)的增量；

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

我們對(duì)誤差函數(shù)進(jìn)行一階近似：

?其中:? , 是每個(gè)誤差函數(shù)對(duì)于x的jacobian;

令 , 得到關(guān)于增量delta_x的線性方程：

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

J_ij只與第i幀和第j個(gè)路標(biāo)點(diǎn)有關(guān)， 只在ii,? ij,? ji, jj四塊處取值；累加而成的H矩陣因此具有如下的稀疏結(jié)構(gòu)：

B塊只與相機(jī)位姿有關(guān)，是對(duì)角陣； C塊只與路標(biāo)點(diǎn)有關(guān)，是對(duì)角陣； E塊非零的地方表示對(duì)應(yīng)的相機(jī)位置有對(duì)對(duì)應(yīng)路標(biāo)點(diǎn)的觀測(cè)；

?2.3 邊緣化（Schur消元）

增量方程可利用H矩陣的特點(diǎn)快速求解：

對(duì)增量方程進(jìn)行劃分：， 并消去地圖點(diǎn)增量：

, 整理得到：

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

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

以上消元的方法，先固定了路標(biāo)點(diǎn)增量，求出位姿增量，再求出路標(biāo)點(diǎn)的增量，從概率論的角度說(shuō)先求了固定路標(biāo)點(diǎn)下的條件概率， 再求路標(biāo)點(diǎn)的邊緣概率，因此稱(chēng)為邊緣化；

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

總結(jié)

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

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