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

• 1. 說明
• 2. Bundle Adjustment(BA)的物理意義
• 3. BA的數學表達
• 4. BA的求解方法
• • 4.1 最速下降法
  • 4.2牛頓法
  • 4.3高斯牛頓法
  • 4.4LM法
  • 4.4 優化方法總結

1. 說明

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

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

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

說的簡單一點： BA的本質是一個優化模型，其目的是減小重投影誤差 。

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

從圖中我們可以看到，長方體的各個頂點分別投影到了 $P_1$ 、$P_2$ 和 $P_3$代表的圖像上。這就是一個普通的投影過程，并沒有什么特殊的地方。但是在SLAM當中，前端的視覺里程計會計算出相鄰兩幅圖像之間的變換關系和地圖點的空間位置。放到上面這幅圖中就是，我們可以計算出 $P_1$ 、$P_2$ 和 $P_3$之間的變換關系和長方體各個定點的空間坐標，但是，不可否認的是我們的計算結果和真實值之間是存在差異的。由于計算誤差的存在，長方體各個頂點的空間坐標與真實坐標并不會完全重合。既然我們計算出的頂點坐標與真實坐標存在差異，那么將我們計算出的頂點位置再次用投影模型投影到圖像上（二次投影，重投影）得到的投影坐標與頂點真實位置的投影（第一次投影）坐標之間不可避免的存在差異，這個誤差我們稱之為 重投影誤差 。

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

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

3. BA的數學表達

投影過程可以分為以下幾個步驟：

1. 首先，把世界坐標轉換到相機坐標，這里將用到相機外參數$(R;t)$：
   $$P^{\prime }=Rp+t=[X^{\prime },Y^{\prime },Z^{\prime }{]}^T$$
2. 然后，將$P^{\prime }$投至歸一化平面，得到歸一化坐標：
   $$P_c=[u_c;v_c;1{]}^T=[X^{\prime }/Z^{\prime };Y^{\prime }/Z^{\prime };1{]}^T$$
3. 對歸一化坐標去畸變，得到去畸變后的坐標。這里暫時只考慮徑向畸變：
   
4. 最后，根據內參模型，計算像素坐標：
   

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

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

然后，把其他時刻的觀測量也考慮進來，我們可以給誤差添加一個下標。那么整體的誤差函數為：
$$\frac{1}{2}\sum _{x=1}^m\sum _{y=1}^n||e_{ij}|{|}^2=\frac{1}{2}\sum _{x=1}^m\sum _{y=1}^n||\widehat{z_{ij}}?h(x_i,y_j)|{|}^2$$

這是一個最小二乘問題，對這個最小二乘進行求解，相當于對位姿和路標同時作了調整，也就是所謂的 BA。

4. BA的求解方法

我們在前面說過，BA是一個優化模型。既然是優化模型，就應該用用各種優化算法來進行計算。我們先來看看各種常用的優化算法。

4.1 最速下降法

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

4.2牛頓法

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

Newton型方法也就是這種思想，首先將函數利用泰勒展開到二次項：
$$f(x+\delta x)=\psi (\delta x)=f(x)+J(x)\delta x+\frac{1}{2}\delta x^{T}H(x)\delta x$$

其中$J$為Jacobi矩陣，對矩陣函數求一次偏導而來，梯度也是對向量函數求一次偏導而來。將標量考慮為1x1的矩陣，將向量考慮nx1的矩陣，其實這些求導都是求Jacobi矩陣。$H$為Hessian矩陣，也就是二次偏導矩陣。

也就是說牛頓方法將函數局部近似成一個二次函數進行迭代，然后令$x$在方向上迭代直至收斂，接下來自然就對這個函數求導了：
$${\psi }^{\prime }(\delta x)=J(x)+H(x)\delta x=0$$
可得
$$\delta x=?H^{?1}J$$

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

4.3高斯牛頓法

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

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

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

4.4LM法

LM方法就是在以上方法基礎上的改進，通過參數的調整使得優化能在最速下降法和高斯牛頓法之間自由的切換，在保證下降的同時也能保證快速收斂。高斯牛頓法最后需要求解的方程為
$$J^{T}J\delta x=?J^T?$$
LM算法在此基礎上做了更改，變成了
$$(J^{T}J+\lambda I)\delta x=?J^T?$$
通過參數 $\lambda$ 的調節在最速下降法和高斯牛頓法之間切換。做個不很數學的直觀分析吧，當 $\lambda$很小時，顯然和高斯牛頓法是一樣的；當 $\lambda$很大時，就變成了這樣：
$$\lambda I\delta x=?J^T?\to \delta x=?{\lambda }^{?1}{J}^T?$$

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

這里還存在一個問題，當 $\lambda$ 取某個值的時候可能會導致 $J+\lambda I$ 不可逆，所以這里變成了
$$(J^{T}J+\lambda diag(J^{T}J))\delta x=?J^T?$$

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

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

4.4 優化方法總結

算法名	特點
最速下降法	每次迭代函數都是下降的，在初始點離最優點很遠時下降速度非?？?#xff0c;但是由于迭代方向是折線形的導致收斂非常慢
牛頓法	收斂速度快，但是Hessian矩陣計算復雜，并不是每次迭代都是下降的
高斯牛頓法	高斯牛頓法就避免了求Hessian矩陣，并且在收斂的時候依舊很快。但是依舊無法保證每次迭代的時候函數都是下降的
LM法	保證了每次迭代都是下降的，并且可以快速收斂

未完待續。。。。。

總結

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

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