當(dāng)前位置：首頁(yè) > 人文社科 > 生活经验 >内容正文

生活经验

ORB_SLAM2代码阅读(5)——Bundle Adjustment

發(fā)布時(shí)間：2023/11/27 生活经验 51 豆豆

生活随笔收集整理的這篇文章主要介紹了 ORB_SLAM2代码阅读(5)——Bundle Adjustment 小編覺得挺不錯(cuò)的,現(xiàn)在分享給大家,幫大家做個(gè)參考.

ORB_SLAM2代碼閱讀（5）——Bundle Adjustment

1. 說明
2. Bundle Adjustment(BA)的物理意義
3. BA的數(shù)學(xué)表達(dá)
4. BA的求解方法
- 4.1 最速下降法
- 4.2牛頓法
- 4.3高斯牛頓法
- 4.4LM法
- 4.4 優(yōu)化方法總結(jié)

1. 說明

前幾篇博客介紹了一下ORB_SLAM2的Tracking、LocalMapping 和 LoopClosing三大線程。本篇博客介紹一下 Bundle Adjustment（簡(jiǎn)稱BA）。本來介紹BA可以單獨(dú)寫一篇文章，不必放在ORB_SLAM2代碼閱讀系列，但是由于ORB_SLAM2中的用到了這種技術(shù)，所以我也就把這部分放到了ORB_SLAM2代碼閱讀系列中。本文將從BA的物理意義、數(shù)學(xué)表達(dá)、求解手段、求解工具和BA在ORB_SLAM2中的應(yīng)用這幾方面來進(jìn)行介紹。

2. Bundle Adjustment(BA)的物理意義

Bundle Adjustment 中文譯為光束法平差。當(dāng)然，它也有其他的中文譯法，比如束調(diào)整、捆集調(diào)整或者捆綁調(diào)整等等。其中bundle，來源于bundle of light，其本意就是指的光束，這些光束指的是三維空間中的點(diǎn)投影到像平面上的光束。而平差則是測(cè)量學(xué)中的概念，指的是：由于測(cè)量?jī)x器的精度不完善和人為因素及外界條件的影響，測(cè)量誤差總是不可避免的。為了提高成果的質(zhì)量，處理好這些測(cè)量中存在的誤差問題，觀測(cè)值的個(gè)數(shù)往往要多于確定未知量所必須觀測(cè)的個(gè)數(shù)，也就是要進(jìn)行多余觀測(cè)。有了多余觀測(cè)，勢(shì)必在觀測(cè)結(jié)果之間產(chǎn)生矛盾，測(cè)量平差的目的就在于消除這些矛盾而求得觀測(cè)量的最可靠結(jié)果并評(píng)定測(cè)量成果的精度。

說的簡(jiǎn)單一點(diǎn)： BA的本質(zhì)是一個(gè)優(yōu)化模型，其目的是減小重投影誤差。

我們可以用一個(gè)模型來表述BA的過程。
這個(gè)圖形經(jīng)常在各種介紹BA的文章中提到，我也在此使用一下這個(gè)圖。

從圖中我們可以看到，長(zhǎng)方體的各個(gè)頂點(diǎn)分別投影到了 $P_{1}$ 、 $P_{2}$ 和 $P_{3}$ 代表的圖像上。這就是一個(gè)普通的投影過程，并沒有什么特殊的地方。但是在SLAM當(dāng)中，前端的視覺里程計(jì)會(huì)計(jì)算出相鄰兩幅圖像之間的變換關(guān)系和地圖點(diǎn)的空間位置。放到上面這幅圖中就是，我們可以計(jì)算出 $P_{1}$ 、 $P_{2}$ 和 $P_{3}$ 之間的變換關(guān)系和長(zhǎng)方體各個(gè)定點(diǎn)的空間坐標(biāo)，但是，不可否認(rèn)的是我們的計(jì)算結(jié)果和真實(shí)值之間是存在差異的。由于計(jì)算誤差的存在，長(zhǎng)方體各個(gè)頂點(diǎn)的空間坐標(biāo)與真實(shí)坐標(biāo)并不會(huì)完全重合。既然我們計(jì)算出的頂點(diǎn)坐標(biāo)與真實(shí)坐標(biāo)存在差異，那么將我們計(jì)算出的頂點(diǎn)位置再次用投影模型投影到圖像上（二次投影，重投影）得到的投影坐標(biāo)與頂點(diǎn)真實(shí)位置的投影（第一次投影）坐標(biāo)之間不可避免的存在差異，這個(gè)誤差我們稱之為重投影誤差。

從上面的過程中我們可以知道，造成重投影誤差的原因是我們計(jì)算的相機(jī)之間的變換關(guān)系和地圖點(diǎn)的空間位置不準(zhǔn)確造成。所以如果我們能夠減小重投影誤差，那么我們就能優(yōu)化相機(jī)的位姿和地圖點(diǎn)的位置。這正是SLAM后端優(yōu)化部分要做的事情。

那么，如何能夠減小重投影誤差就是BA要做的事情了。

3. BA的數(shù)學(xué)表達(dá)

投影過程可以分為以下幾個(gè)步驟：

首先，把世界坐標(biāo)轉(zhuǎn)換到相機(jī)坐標(biāo)，這里將用到相機(jī)外參數(shù) $(R; t)$ ：
$=[X',Y',Z']^T$
然后，將 $P^{'}$ 投至歸一化平面，得到歸一化坐標(biāo)：
$Pc=[uc;vc;1]T=[X′/Z′;Y′/Z′;1]TP_{c} = [u_{c}; v_{c}; 1]^T = [X'/Z'; Y'/Z'; 1]^T$
對(duì)歸一化坐標(biāo)去畸變，得到去畸變后的坐標(biāo)。這里暫時(shí)只考慮徑向畸變：
最后，根據(jù)內(nèi)參模型，計(jì)算像素坐標(biāo)：

以上四步可以用一個(gè)方程表示，即
$z = h (x, y)$
其中， $z$ 表示像素坐標(biāo) $u_s,v_s)$ ， $x$ 表示外參 $(R, t)$ ， $y$ 表示空間點(diǎn) $p$ 。

那么重投影誤差我們就可以表示為：
$e=z^?h(x,y)e = \hat{z}-h(x,y)$
其中 $z^\hat{z}$ 表示重投影坐標(biāo)

然后，把其他時(shí)刻的觀測(cè)量也考慮進(jìn)來，我們可以給誤差添加一個(gè)下標(biāo)。那么整體的誤差函數(shù)為：
$12∑x=1m∑y=1n∣∣eij∣∣2=12∑x=1m∑y=1n∣∣zij^?h(xi,yj)∣∣2\frac{1}{2} \sum^{m}_{x = 1}\sum^{n}_{y=1}{||e_{ij}||^{2}} = \frac{1}{2} \sum^{m}_{x = 1}\sum^{n}_{y=1}{||\hat{z_{ij}}-h(x_i,y_j)||^{2}}$

這是一個(gè)最小二乘問題，對(duì)這個(gè)最小二乘進(jìn)行求解，相當(dāng)于對(duì)位姿和路標(biāo)同時(shí)作了調(diào)整，也就是所謂的 BA。

4. BA的求解方法

我們?cè)谇懊嬲f過，BA是一個(gè)優(yōu)化模型。既然是優(yōu)化模型，就應(yīng)該用用各種優(yōu)化算法來進(jìn)行計(jì)算。我們先來看看各種常用的優(yōu)化算法。

4.1 最速下降法

如果對(duì)梯度比較熟悉的話，那應(yīng)該知道梯度方向是函數(shù)上升最快的方向，而此時(shí)我們需要解決的問題是讓函數(shù)最小化。你應(yīng)該想到了，那就順著梯度的負(fù)方向去迭代尋找使函數(shù)最小的變量值。梯度下降法就是用的這種思想，用數(shù)學(xué)表達(dá)：
$xk=xk?1?λΔf(xk?1)x_k = x_{k-1} - \lambda\Delta f(x_{k-1})$
其中 $λ\lambda$ 為步長(zhǎng)。最速下降法保證了每次迭代函數(shù)都是下降的，在初始點(diǎn)離最優(yōu)點(diǎn)很遠(yuǎn)的時(shí)候剛開始下降的速度非常快，但是最速下降法的迭代方向是折線形的導(dǎo)致了收斂非常非常的慢。

4.2牛頓法

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

Newton型方法也就是這種思想，首先將函數(shù)利用泰勒展開到二次項(xiàng)：
$f(x+δx)=ψ(δx)=f(x)+J(x)δx+12δxTH(x)δxf(x+\delta x) =\psi (\delta x)= f(x)+J(x)\delta x + \frac{1}{2}\delta x^TH(x)\delta x$

其中 $J$ 為Jacobi矩陣，對(duì)矩陣函數(shù)求一次偏導(dǎo)而來，梯度也是對(duì)向量函數(shù)求一次偏導(dǎo)而來。將標(biāo)量考慮為1x1的矩陣，將向量考慮nx1的矩陣，其實(shí)這些求導(dǎo)都是求Jacobi矩陣。 $H$ 為Hessian矩陣，也就是二次偏導(dǎo)矩陣。

也就是說牛頓方法將函數(shù)局部近似成一個(gè)二次函數(shù)進(jìn)行迭代，然后令 $x$ 在方向上迭代直至收斂，接下來自然就對(duì)這個(gè)函數(shù)求導(dǎo)了：
$ψ′(δx)=J(x)+H(x)δx=0\psi '(\delta x) = J(x) + H(x)\delta x = 0$
可得
$δx=?H?1J\delta x = -H^{-1}J$

牛頓型方法收斂的時(shí)候特別快，尤其是對(duì)于二次函數(shù)而言一步就可以得到結(jié)果。但是該方法有個(gè)最大的缺點(diǎn)就是Hessian矩陣計(jì)算實(shí)在是太復(fù)雜了，并且牛頓型方法的迭代并不像最速下降法一樣保證每次迭代都是下降的。

4.3高斯牛頓法

既然牛頓法計(jì)算Hessian矩陣太困難了，那有沒有什么方法可以不計(jì)算Hessian矩陣呢？將泰勒展開式的二次項(xiàng)也去掉好像就可以避免求Hessian矩陣了吧，就像這樣：
$f(x+δx)=f(x)+J(x)δxf(x+\delta x) = f(x)+J(x)\delta x$
這好像變成了一個(gè)線性函數(shù)了啊，線性函數(shù)如果要最小化的話好像是需要增加其他的約束條件的啊。那這里有沒有其他的約束條件呢？仔細(xì)思考一下，我們需要最小化的是重投影誤差，它的最小值是什么呢？理想狀態(tài)下當(dāng)然是等于0了。所以這個(gè)時(shí)候就不應(yīng)該求導(dǎo)了，而是直接令函數(shù)為0。此時(shí)，令 $=\epsilon$ 有
$?+J(x)δx=0\epsilon+J(x)\delta x = 0$
即
$JTJδx=?JT?J^TJ \delta x = -J^T \epsilon$
$\delta x$

由此x在 $δx\delta x$ 方向上迭代直至 $∣∣?∣∣||\epsilon||$ 最小。

高斯牛頓法就避免了求Hessian矩陣，并且在收斂的時(shí)候依舊很快。但是依舊無法保證每次迭代的時(shí)候函數(shù)都是下降的。

4.4LM法

LM方法就是在以上方法基礎(chǔ)上的改進(jìn)，通過參數(shù)的調(diào)整使得優(yōu)化能在最速下降法和高斯牛頓法之間自由的切換，在保證下降的同時(shí)也能保證快速收斂。高斯牛頓法最后需要求解的方程為
$JTJδx=?JT?J^TJ \delta x = -J^T \epsilon$
LM算法在此基礎(chǔ)上做了更改，變成了
$(JTJ+λI)δx=?JT?(J^TJ+\lambda I) \delta x = -J^T \epsilon$
通過參數(shù) $λ\lambda$ 的調(diào)節(jié)在最速下降法和高斯牛頓法之間切換。做個(gè)不很數(shù)學(xué)的直觀分析吧，當(dāng) $λ\lambda$ 很小時(shí)，顯然和高斯牛頓法是一樣的；當(dāng) $λ\lambda$ 很大時(shí)，就變成了這樣：
$λIδx=?JT?→δx=?λ?1JT?\lambda I \delta x = -J^T \epsilon \rightarrow \delta x = -\lambda ^{-1}J^T \epsilon$

這和最速下降法的形式一致。

這里還存在一個(gè)問題，當(dāng) $λ\lambda$ 取某個(gè)值的時(shí)候可能會(huì)導(dǎo)致 $J+λIJ+\lambda I$ 不可逆，所以這里變成了
$(JTJ+λdiag(JTJ))δx=?JT?(J^TJ+\lambda diag(J^TJ)) \delta x = -J^T \epsilon$

其實(shí)LM算法的具體形式就筆者看到的就有很多種，但是本質(zhì)都是通過參數(shù) $λ\lambda$ 在最速下降法和高斯牛頓法之間切換。

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

4.4 優(yōu)化方法總結(jié)

算法名	特點(diǎn)
最速下降法	每次迭代函數(shù)都是下降的，在初始點(diǎn)離最優(yōu)點(diǎn)很遠(yuǎn)時(shí)下降速度非常快，但是由于迭代方向是折線形的導(dǎo)致收斂非常慢
牛頓法	收斂速度快，但是Hessian矩陣計(jì)算復(fù)雜，并不是每次迭代都是下降的
高斯牛頓法	高斯牛頓法就避免了求Hessian矩陣，并且在收斂的時(shí)候依舊很快。但是依舊無法保證每次迭代的時(shí)候函數(shù)都是下降的
LM法	保證了每次迭代都是下降的，并且可以快速收斂

未完待續(xù)。。。。。

總結(jié)

以上是生活随笔為你收集整理的ORB_SLAM2代码阅读(5)——Bundle Adjustment的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問題。

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