因子圖優(yōu)化原理（iSAM、iSAM2）

• slam問題
• 通過貝葉斯網(wǎng)絡(luò)對slam問題建模
• 從貝葉斯網(wǎng)絡(luò)到因子圖
• 非線性最小二乘問題求解
• isam1增量QR分解
• isam2
• 結(jié)語

slam問題

在介紹因子圖之前，先從一個簡單的slam問題入手，如下圖所示：

在圖中清晰的顯示了各個節(jié)點(diǎn)和和連接結(jié)點(diǎn)邊之間的定義，對于圖結(jié)構(gòu)不做過多說明，假定讀者已經(jīng)有一定的先驗知識。在上圖顯示的slam問題中，實(shí)際上我們要解決的問題就是，從機(jī)器人的控制和觀測中恢復(fù)機(jī)器人的路徑以及環(huán)境地圖。

通過貝葉斯網(wǎng)絡(luò)對slam問題建模

在貝葉斯網(wǎng)絡(luò)中，通過有向邊連接變量，除了路標(biāo)狀態(tài)節(jié)點(diǎn)和機(jī)器人的狀態(tài)節(jié)點(diǎn)外（兩個狀態(tài)節(jié)點(diǎn)為狀態(tài)變量），還定義了路標(biāo)觀測節(jié)點(diǎn)（紅色）和運(yùn)動觀測節(jié)點(diǎn)（綠色）（兩個觀測節(jié)點(diǎn)為觀測變量）。這個貝葉斯實(shí)際上描述了一種狀態(tài)和觀測的聯(lián)合概率模型。假設(shè)知道系統(tǒng)的狀態(tài)變量X（機(jī)器人所在位置、路標(biāo)點(diǎn)所在位置），可以推測出機(jī)器人得到的觀測量Z（觀測量是通過某些傳感器得到的，而傳感器有已知模型，可以通過數(shù)據(jù)手冊得到），即已知機(jī)器人的狀態(tài)量和傳感器的模型，就可以推算出機(jī)器人的觀測量。所以得到了如下圖所示的一個生成模型：

由于觀測量之間相互獨(dú)立，可以得到下式所述的模型：

從貝葉斯網(wǎng)絡(luò)到因子圖

基于上文，我們知道貝葉斯網(wǎng)絡(luò)是一個生成模型，解決的從狀態(tài)變量得到觀測觀測變量，而我們的問題是從觀測變量來推出狀態(tài)變量，是一個狀態(tài)估計問題，這就引出了因子圖，因子圖是一個inference model。
首先來對問題做一個推導(dǎo)：由于我們要從觀測量來推出狀態(tài)量，這是一個后驗概率問題，結(jié)合貝葉斯丁玲，問題被描述為最大化一個后驗概率問題（MAP），即給定系統(tǒng)系統(tǒng)觀測量，求解系統(tǒng)狀態(tài)量，使得下式的條件概率最大：

通過上面的定義將MAP問題轉(zhuǎn)換成求解一個最優(yōu)的狀態(tài)，使得似然概率和先驗概率的乘積最大化。

因子的定義推導(dǎo)：將先驗和似然項因式分解作為因子：

將上述乘積形式的每一項表示成因子，將狀態(tài)變量表示成節(jié)點(diǎn)，觀測量表示成邊，就轉(zhuǎn)化成了下面的因子圖。

對于上面的因子圖，我們用下面的式子來描述：

其中紅色的部分表示觀測因子，綠色為里程計因子，紫色為先驗因子。那我們要解決的問題是什么呢，就是尋找一個最優(yōu)的X，使得這些因子圖的乘積最大化。

因子圖中每個因子的定義：在因子圖中通常每個因子都是通過指數(shù)函數(shù)的形式來定義的，這是因為我們得到的觀測量是有不確定性的，而且現(xiàn)實(shí)中這種不確定性通常服從高斯分布：

其中f(x)表示的是誤差函數(shù)，當(dāng)誤差越小的時候，其指數(shù)函數(shù)越大，其定義如下圖所示：

通過上面的說明，到這里我們終于得到了我們定義因子圖之后要解決的問題了。我們先理一遍，首先我們介紹了貝葉斯網(wǎng)絡(luò)，他是一個概率網(wǎng)絡(luò)，通過貝葉斯網(wǎng)絡(luò)可以解決已知狀態(tài)變量來推出觀測變量的問題，但是事實(shí)是我們通常可以得到觀測變量，想要推出狀態(tài)變量，所以我們推出了因子圖，并且結(jié)合貝葉斯定理，將問題描述成一個最大后驗概率問題。最終得到目標(biāo)函數(shù)：

現(xiàn)在有了目標(biāo)函數(shù)之后，剩下的最后一個問題就是，如何求解這個最優(yōu)化問題。對于目標(biāo)函數(shù)的右邊取負(fù)對數(shù)，可將最大化因子的成績問題轉(zhuǎn)化成一個非線性最小二乘問題：

通過求解這個非線性最小二乘的問題就能得到問題的最優(yōu)解。

非線性最小二乘問題求解

對于上式的最小二乘問題，可以通過高斯牛頓迭代法來求解，首先給定X一個初始值（通常在slam問題中這個初始值我們使用上一時刻得到的最有狀態(tài)估計X），在給定這個變量的基礎(chǔ)上再去求一個修改量，最后在用這個初始值加上這個修改量得到新的狀態(tài)X，重復(fù)迭代這個過程，直到收斂。其流程如下圖所示：

對于上圖中的非線性最小二乘問題，對中間的式子做一階泰勒展開，就可以將其轉(zhuǎn)化為線性最小二乘問題：

現(xiàn)在的問題就轉(zhuǎn)化成了如何求解這個線性最小二乘問題，對于一個線性最小二乘問題是由比較成熟的方法來得到線性最小而成問題的最優(yōu)解，比較常見的方法為Normal equation：

對于Normal equation問題可以通過Cholesky分解來求解：

也可以通過QR分解來求解：

下面通過最開始的簡單的slam問題來說明一下因子圖的求解過程：

上圖中的雅可比矩陣的每一行對應(yīng)一個因子，右邊的矩陣是一個信息矩陣，在現(xiàn)實(shí)中生成的因子圖所獲得的雅可比矩陣是稀疏矩陣，這是一個很好的性質(zhì)，非常有利于線性系統(tǒng)的求解：

但是這個矩陣的稀疏性和變量的order有關(guān)，order不一樣的話，矩陣的稀疏性體現(xiàn)也不一樣，會出現(xiàn)下圖所示的情況：

這種情況下不利于線性系統(tǒng)的求解，所以通常需要一些算法來對信息矩陣進(jìn)行重排序來維持其好的稀疏性，目前較為成熟的算法為COLAMD。通過上面的描述我們知道了對于一個確定的因子圖如何求解。但是常見的問題（比如SLAM問題）生成的因子圖是增量的，并且在機(jī)器人運(yùn)行的過程中，因子圖只是又很小一部分的改變，如果我們從頭對因子圖進(jìn)行分解來求解的話，不利于系統(tǒng)的實(shí)時性，那么如何解決這個問題呢？下面介紹論文isam中是怎么做的。.

isam1增量QR分解

對于給定的J進(jìn)行QR分解如下圖所示：

現(xiàn)在我想對J加入幾行，這幾行的意義就是，當(dāng)因子圖中加入了新的節(jié)點(diǎn)和因子時在信息矩陣中的表現(xiàn)就是多了幾行，問題就是怎么不在重新分解的基礎(chǔ)上能得到新的R。

論文中給出了求解方法，公式如下圖所示：

對于在R下面加了一行（非零元素）變成如下圖所示：

其求解辦法是通過givens rotation可以將R變成一個新的上三角矩陣：

但是隨著因子的增加，也會帶來一個問題，就是如果我新加的因子只影響上一時刻的因子的時候，這個矩陣依然是有很好的稀疏性，但是如果新加入的因子是回環(huán)，那么givens rotation講影響之前的所有節(jié)點(diǎn)，使R矩陣變得稠密，如下圖所示

這時候就體現(xiàn)出了isam1的問題，在經(jīng)過以一定的時間后就需要對元素進(jìn)行重新排序，將稠密矩陣變得稀疏：

isam2

在這里簡單說明isam2是將因子圖轉(zhuǎn)換成貝葉斯網(wǎng)絡(luò)去求解，具體的文章參考paper吧，我自己還沒有研究透徹。但是我建議明白了因子圖的求解流程之后，就不要再去看那枯燥的公式了，腦瓜子疼。其實(shí)他就是求解問題的一種工具，所以明白了問題之后，調(diào)庫不香嘛？

結(jié)語

寫這篇文章是因為，自己早早的就接觸到了因子圖優(yōu)化，針對salm問題的因子圖優(yōu)化問題，我就一直想不通，這個后驗概率是在哪里體現(xiàn)的呢，現(xiàn)在終于明白了，其實(shí)就是上面指數(shù)形式的因子推導(dǎo)的過程，雅可比矩陣是什么玩意呢？原來是矩陣的一階倒數(shù)呀。所以大家在學(xué)習(xí)論文的時候不要害怕公式，因為公式看不懂你是真不懂，哈哈。

總結(jié)

以上是生活随笔為你收集整理的因子图优化原理（iSAM、iSAM2论文解析）的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

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