目錄

• iSAM
• • 主要參考論文
  • 因子圖
  • 問題轉化
  • 增量式因子圖構建與問題求解
• iSAM2
• • 主要參考論文
  • 一些鋪墊
  • 貝葉斯樹
• 總結

iSAM

主要參考論文

論文名iSAM: Incremental Smoothing and Mapping
作者Michael Kaess, Student Member, IEEE, Ananth Ranganathan, Student Member, IEEE,and Frank Dellaert, Member, IEEE
期刊： 2008 T-RO

因子圖

因子圖也是一種圖結構，由頂點和邊組成，頂點就是我們的待優(yōu)化的變量，或者參數，對應批量優(yōu)化就是機器人的狀態(tài)變量和環(huán)境中需要定位的landmark參數。這里的邊描述為連接兩個參數的因子，物理意義為當前兩種參數狀態(tài)下的觀測，可以是連接兩個機器人狀態(tài)量的里程計信息（雷達視覺imu里程計等），也可以指機器人狀態(tài)到環(huán)境變量的觀測。

問題轉化

因子圖的優(yōu)化目標是圖中所有狀態(tài)參數變量，使得所有觀測因子的連乘結果最大，每個因子認為服從高斯分布。問題描述如下：

$$\begin{gathered} f_i\left({\Theta }_i\right)\propto \exp \left(?\frac{1}{2}{\Vert h_i\left({\Theta }_i\right)?z_i\Vert }_{{\Sigma }_i}^2\right) \\ f(\Theta )=\prod _i{f}_i\left({\Theta }_i\right) \\ {\Theta }^{?}=\underset{\Theta }{\mathrm{argmax}}f(\Theta ) \end{gathered}$$

將問題的目標函數在初始的參數位置進行線性化得到如下表達式：

$$\underset{\Delta }{\mathrm{argmin}}(?\log f(\Delta ))=\underset{\Delta }{\mathrm{argmin}}\Vert A\Delta ?\mathbf{b}{\Vert }^2$$

這樣就將非線性最小二乘問題轉化成了線性最小二乘問題，注意到其中的：

$$A\in {\mathbb{R}}^{m\times n}$$

m為測量行 n為變量數，對于因子圖問題每多一個新的狀態(tài)量一定會多至少一個因子，因此m總是大于n的。A是長條形的，因此對A進行QR分解得到:

$$A=Q\left[\begin{array}{l}R\\ 0\end{array}\right]$$

最小二乘問題化簡成：

$$\begin{array}{rl}\Vert A\mathbf{\theta }?\mathbf{b}{\Vert }^2& ={\Vert Q\left[\begin{array}{c}R\\ 0\end{array}\right]\mathbf{\theta }?\mathbf{b}\Vert }^2\\ & ={\Vert Q^{T}Q\left[\begin{array}{c}R\\ 0\end{array}\right]\mathbf{\theta }?Q^T\mathbf{b}\Vert }^2\\ & ={\Vert \left[\begin{array}{c}R\\ 0\end{array}\right]\mathbf{\theta }?\left[\begin{array}{l}\mathbf{d}\\ \mathbf{e}\end{array}\right]\Vert }^2\\ & =\Vert R\mathbf{\theta }?\mathbf{d}{\Vert }^2+\Vert \mathbf{e}{\Vert }^2\end{array}$$

后半部分為常數部分，前半部分為二次型，最小值為零，因此最小二乘問題被轉換成上式前半部分的線性方程組的解。

增量式因子圖構建與問題求解

如上理論推導是為非線性最小二乘的共性問題，iSAM主要提出了增量更新方法：

$$\left[\begin{array}{ll}{Q}^T& \\ & 1\end{array}\right]\left[\begin{array}{c}A\\ {\mathbf{w}}^T\end{array}\right]=\left[\begin{array}{c}R\\ {\mathbf{w}}^T\end{array}\right]\text{,?new?rhs:?}\left[\begin{array}{c}\mathbf{d}\\ \gamma \end{array}\right]$$

當獲得了新的測量行時雅克比矩陣A下方多了一行新測量因子線性化項（行），該線性化項（行）列數可以比A多，用0補齊A右側即可，且是稀疏的，因為僅有特定的參數狀態(tài)（列）與該因子有關，當新的增量雅克比矩陣左乘原正交矩陣時（相差的行列用單位矩陣補齊，此時仍是正交的）就可以得到R矩陣的增量表達，增量方式與A相同，注意此時新增的行依然是稀疏的。

時只需要對R矩陣進行Givens旋轉變換就可以將下面的非零項消除，得到新的R矩陣:

$$\left[\begin{array}{c}{R}^{\prime }\\ 0\end{array}\right]$$

這樣就完成了一次增量更新，我們注意到使用iSAM在這種情況下可以只對新增的測量因子線性化，也不需要對雅可比矩陣A重新QR分解，就得到了我們想要的結果，這一過程像濾波算法一樣簡單，但卻可以得到批量優(yōu)化一樣好的全局最優(yōu)效果。這就是iSAM的閃光點。

是不是能一直這樣方便下去呢？答案實際是不行的，我們得到如此好的結果是基于R矩陣是稀疏的前提條件。Givens旋轉是在n維空間中旋定兩組正交基張成的空間中旋轉。

也就是說在消去R底部對應行的過程中，上面的某一行也在發(fā)生著改變，稀疏性會逐漸的遭到破壞如下圖所示：

因此需要周期性的重排序，來恢復矩陣的稀疏性。這一過程則極為耗時。

因此iSAM拋出問題：如何減少重排序的次數，如何降低新增狀態(tài)量，對R矩陣稀疏性的破壞程度。

iSAM2

主要參考論文

論文名iSAM2: Incremental smoothing and mapping using the Bayes tree
作者Michael Kaess, Hordur Johannsson, Richard Roberts, Viorela Ila, John Leonard, and Frank Dellaert
期刊： 2012 IJRR

一些鋪墊

這篇文章的核心在于引入了貝葉斯樹，從而解決了減少了或者優(yōu)化了，iSAM1中遺留下來的問題。那么什么是貝葉斯樹呢？

定義一下文章中說到的術語：
消元/消去：代表由因子圖轉換為貝葉斯網絡的過程中一步操作，將因子圖中的一個節(jié)點和相連的因子邊轉換為貝葉斯網絡中的節(jié)點和有向邊的過程。

首先我認為貝葉斯樹本身并沒有特別的意義，真正的價值在于你維護樹的過程，當你按照貝葉斯樹的法則去維護這些變量和狀態(tài)的關系時，你會驚訝的發(fā)現(xiàn)你每次求解問題的規(guī)模都是小的，求解的矩陣都是稀疏的，最終你的問題過程都是容易解決的。就像求解方程組，再好的算法都很難從時間復雜度上簡化求解方程組。但是好的算法你會發(fā)現(xiàn)每一步求解都是十分easy的?；蛘呦穸雅判蛩惴?#xff0c;關鍵在于你維護這個堆的過程。

因子圖定義和前面的文章一樣并沒有變化。但是貝葉斯樹是基于貝葉斯網絡演變過來的，貝葉斯網絡可以參考貝葉斯網專題2：貝葉斯網基本概念_吳智深的博客-CSDN博客.這里面有很細致的介紹。

許多概率問題直接使用聯(lián)合分布進行不確定性推理的復雜度很高。因此，聯(lián)合分布的復雜度與變量個數呈指數增長。當變量很多時，聯(lián)合概率的獲取、存儲和運算都變得十分困難。

如果通過連規(guī)則將聯(lián)合概率分布分解為條件概率乘積的形式，再根據變量之間的獨立性進行化簡，問題就會簡化很多。最后將變量之間的依賴關系用有向箭頭表示就得到了貝葉斯網。
$$\begin{gathered} \begin{array}{rl}& P(B,E,A,J,M)\\ =& P(B)P(E\mid B)P(A\mid B,E)P(J\mid B,E,A)P(M\mid B,E,A,J)\end{array} \\ P(B,E,A,J,M)=P(B)P(E)P(A\mid B,E)P(J\mid A)P(M\mid A) \end{gathered}$$

首先求解因子圖模型可以理解為概率推理的過程，每個因子本身可以看作是在當前觀測條件下，關聯(lián)的變量聯(lián)合概率分布。因此用貝葉斯網絡進行替換也是高效的。

因子圖可以轉換成貝葉斯網絡。因子圖問題中其實沒有因果關系，但是依然可以表達成條件概率乘以邊緣概率的形式，形如下式。
$$f_{\text{joint?}}\left({\theta }_j,S_j\right)=P\left({\theta }_j\mid S_j\right)f_{\text{new?}}\left(S_j\right)$$
邊緣概率可能描述的不準確，因為分解只是對部分因子進行操作，并不代表已經包含了所有關于邊緣某變量的因子，因此、分解得到的條件變量的因子$f_{\text{new?}}\left(S_j\right)$并不等于最終該變量的邊緣概率分布。所以這個$f_{\text{new?}}\left(S_j\right)$暫時并不具備實際意義，只是一個形如高斯分布的函數而已。

但是前面的$P\left({\theta }_j\mid S_j\right)$卻是實實在在的前向變量的條件概率密度，為什么呢？
這就需要介紹一下分解的過程了，也就是從因子圖生成貝葉斯網絡的過程。
這個是因子圖

變量還是那些變量，邊變成了有向邊，邊的順序與變量的消元順序息息相關。我們先從圖的角度描述消元過程介紹是怎么由第一張圖變到第二張圖的。下圖是論文中給出的完整消元過程，作者給出的消元順序為$l_1{l}_2{x}_1{x}_2{x}_3$
以$l_1$為例首先將和變量$l_1$相連的所有因子邊斷開，替換成指向$l_1$的有向邊，然后所有與變量$l_1$相連的狀態(tài)或變量之間兩兩添加一條因子邊，如果只有單個變量則添加一個因子（消除$l_2$時$x_3$添加的紅色點線），這些因子可以和原有的因子結合起來（連乘）。這樣逐個消去就是圖視角的變換。

這個過程和QR分解是一致的，我們先從外觀上對比一下。


(a)A為因子模型的雅可比矩陣，行為測量因子，列為狀態(tài)變量，每個因子只和部分狀態(tài)向量是相關聯(lián)的，因此A矩陣也是稀疏的。關聯(lián)性自然和圖模型是一致對應的。

(b)R矩陣為A矩陣QR分解或者Cholesky分解得到的上三角矩陣，對角元素為該行所條件概率密度$P\left({\theta }_j\mid S_j\right)$對應的狀態(tài)變量前向變量，同行剩下的元素對應條件概率密度中的條件變量。也可以看出二者是一致對應的。類比一下求解方程組時，從下往上依次回代求解的過程，與聯(lián)合概率密度函數鏈分解后求最大分布的過程是一樣的，我們也是先從邊緣概率求極值然后將得到的取值作為條件回代到下一個，以該變量為條件的條件概率密度中，然后再求極值。舉個例子：
$$P(B,E,A,J,M)=P(B)P(E)P(A\mid B,E)P(J\mid A)P(M\mid A)$$
式中BE事件是獨立的且已知分布，因此可以直接對BE密度函數求極值，線性化之后就是很簡單的二次型很好求解。在已知BE分布之后A的條件概率密度就確定了，也是一個簡單的二次型。如此遞推。這和求方程組的變量回代過程是一樣的。

思考一下為什么一樣，這就要看文中所提出來的因子分解方法。也就是說消元的過程中實際上對因子函數做了什么。

待分解因子$$f_{\text{joint?}}\left({\Delta }_j,{\mathbf{s}}_j\right)\propto \exp \left\{?\frac{1}{2}{\Vert \mathbf{a}{\Delta }_j+{\mathbf{A}}_S{\mathbf{s}}_j?\mathbf{b}\Vert }^2\right\}$$為與待消除變量相連的全部因子的連乘獲得的，前面已經介紹過。

先給出結論分解出的條件概率，邊緣因子解析表達式如下：
$$\begin{gathered} f_{\text{joint?}}\left({\theta }_j,S_j\right)=P\left({\theta }_j\mid S_j\right)f_{\text{new?}}\left(S_j\right) \\ P\left({\Delta }_j\mid {\mathbf{s}}_j\right)\propto \exp \left\{?\frac{1}{2}{\left({\Delta }_j+{\mathbf{rs}}_j?d\right)}^2\right\} \\ {f}_{new}\left({\mathbf{s}}_j\right)=\exp \left\{?\frac{1}{2}{\Vert {\mathbf{A}}^{\prime }{\mathbf{s}}_j?{\mathbf{b}}^{\prime }\Vert }^2\right\} \end{gathered}$$
其中$\mathbf{r}?{\mathbf{a}}^{?}{\mathbf{A}}_S,d?{\mathbf{a}}^{?}\mathbf{b},{\mathbf{a}}^{?}?{\left({\mathbf{a}}^T\mathbf{a}\right)}^{?1}{\mathbf{a}}^T$
其中第二項是通過將第一項得到的變量增量的最優(yōu)解析表達${\Delta }_j=d?{\mathbf{rs}}_j$回代回因子中得到的余項。也就是要傳遞給貝葉斯網絡中父節(jié)點的邊緣因子。（對應圖視角下的相連的狀態(tài)或變量之間兩兩添加一條因子邊的過程。）

原文中只是給出了解析表達并沒有附上詳細的證明，筆者嘗試證明了一下：
待求最小值的二次型為${\Vert \mathbf{a}{\Delta }_j+{\mathbf{A}}_S{\mathbf{s}}_j?\mathbf{b}\Vert }^2$
$$={\Vert \left[\frac{\mathbf{a}}{|\mathbf{a}|}{\mathbf{a}}_1\dots {\mathbf{a}}_{m_j?1}\right]\left[\begin{array}{c}|\mathbf{a}|\\ 0\end{array}\right]{\Delta }_j+A_s{\mathbf{s}}_j?\mathbf{b}\Vert }^2$$

其中${\mathbf{a}}_{m_j?1}$為補全的單位正交基

上式左乘一個單位正交矩陣不影響其二范數取值，那么就左乘剛剛提取出來的正交矩陣的轉置。
$$\begin{gathered} =\Vert \left[\begin{array}{c}|\mathbf{a}|\\ 0\end{array}\right]{\Delta }_j+?\begin{array}{c}\frac{{\mathbf{a}}^T}{|\mathbf{a}|}\\ {\mathbf{a}}_1^T\\ ?\\ {\mathbf{a}}_{m_j?1}^T\end{array}?A_S{s}_j??\begin{array}{c}\frac{{\mathbf{a}}^T}{|\mathbf{a}|}\\ {\mathbf{a}}_1^T\\ ?\\ {\mathbf{a}}_{m_j?1}^T\end{array}?\Vert {\Vert }^2 \\ ={\Vert |\mathbf{a}|{\Delta }_j+\frac{{\mathbf{a}}^{\mathbf{T}}}{|\mathbf{a}|}{A}_S{s}_j?\frac{{\mathbf{a}}^{\mathbf{T}}}{|\mathbf{a}|}\mathbf{b}\Vert }^2+{\Vert ?\begin{array}{c}{\mathbf{a}}_1{}^T{A}_s{\mathbf{s}}_j\\ ?\\ {\mathbf{a}}_{m_j?1}{}^T{A}_s{\mathbf{s}}_j\end{array}???\begin{array}{c}{\mathbf{a}}_1{}^T\mathbf{b}\\ ?\\ {\mathbf{a}}_{m_j?1}{}^T\mathbf{b}\end{array}?\Vert }^2 \end{gathered}$$
左邊是與當前待求參數相關的數值平方項，右面是與當前待求參數無關的二范數項，分別對應條件概率密度（未歸一化）和邊緣因子。與作者所描述分解方法相符。

從上述推導可以看出，由因子圖轉換成貝葉斯網絡的過程中，每一步實際在做的是一步QR分解，宏觀上消元順序對應QR分解順序。

到此就基本解釋了圖視角和矩陣變換視角的對應關系，以及其對應的概率分解運算步驟。

貝葉斯樹

首先建議先看一下團樹，因為作者反復提到貝葉斯樹是與團樹或者聯(lián)結樹由很高的相似性并且從中發(fā)展來的。貝葉斯網專題6：團樹傳播_吳智深的博客-CSDN博客_團樹傳播算法.

團樹與貝葉斯樹一樣，將變量集合存成節(jié)點，集合之間可以有重復變量，兩個包含相同變量的節(jié)點之間一定是連通的。不同的是貝葉斯樹是有向的。

團樹的優(yōu)勢之一在于推理的復用，舉個例子一個概率模型中包含甲乙丙丁四個變量，我推理完甲的條件概率，當我需要推理乙的條件概率時，可以復用甲推理過程中的部分中間推理結果。使用團樹可以很清晰的維護這些中間結果，方便信息傳遞以及推理復用。

當我們求解因子圖問題時，新增的變量和測量難免會改變模型結構，但是并不一定會直接影響全局的貝葉斯網絡結構，還用論文中的例子：當$x_2,x_3$之間添加了新的測量，其實并不影響$l_2$的消元過程，也不改變$l_2$向$x_3$傳遞的邊緣因子。因此貝葉斯樹也被用來存儲一定的復用關系，當子樹中變量未受影響時，子樹結構和推理結果（從子樹中傳遞上來的邊緣因子）都不發(fā)生改變。貝葉斯樹的生成方式偽代碼附在下面。

總結來說，就是逆著消元的過程檢索生成貝葉斯樹。如果某貝葉斯網絡中的節(jié)點（變量）完全以其父團為條件變量，那就加入父團，如果不“完全”就新建一個子團，父子團之間使用有向邊連接。個人理解貝葉斯樹代表著QR矩陣中的最小稠密單元。還是看文中提到的例子，按照作者的方法生成的貝葉斯樹如左圖所示，對應變量在矩陣中的關系如右圖所示。R矩陣是稀疏的，將R矩陣拆成一個個稠密的最小單元得到的就是貝葉斯樹。每個樹節(jié)點（變量團）都是完全稠密的。其中冒號表示分隔關系，冒號前為前向變量，冒號后為條件變量，文中叫分隔變量。

當新增測量因子時，比如當$x_2,x_3$之間產生了共視關系。那么含有$x_2$的團$l_1,x_1:x_2$

及其父團都將受到影響，如圖所示圈中的團受到影響。而綠色的子樹部分沒有受到影響。

增量更新時，將受影響的樹頂部分刪除，并重建因子圖，然后選擇最優(yōu)的消元順序重新生成貝葉斯樹，然后將子樹連接回來。

這里說明了貝葉斯樹是更好的維持了與線性代數的等價性，并使算法支持遞歸估計，可并行計算。

整個邏輯推理其實可以大致分為兩個過程，第一步是從貝葉斯網絡葉子節(jié)點向其根部的因子分解過程，這一步對應因子圖生成貝葉斯網的消元過程，每次都將與當前節(jié)點（變量）無關的分離出來的邊緣因子傳遞給其父節(jié)點，稱之為由下到上的過程；第二步是從父節(jié)點開始依次計算出當前節(jié)點的最優(yōu)增量值大小，其已經由第一步消元過程中計算出了解析表達${\Delta }_j=d?{\mathbf{rs}}_j$只需要回代即可，稱之為由上到下的過程。

我們注意到當變量的增量值已經很大時，是需要重線性化的，文中使用閾值來判斷哪些變量需要重線性化。同時使用另一個閾值來判斷，重新線性化后或者添加了新的變量或者測量之后，第二步，從上到下計算增量，的過程是否需要重新計算（因為條件變量的取值變了，但是如果增量變化很小的話，就可以停止這個計算過程。下面的子樹中的增量就也不需要更新了）。這兩步屬于加速算法的trick，利用的是，slam問題新的觀測往往對較遠的狀態(tài)估計產生很小的影響。

此外就是每次貝葉斯樹的生成過程中，消元順序的選擇，這里會最終影響貝葉斯樹的結構。求最優(yōu)消元順序實際上是NP難的問題，一般使用啟發(fā)式的方法求解。

文中介紹了一種啟發(fā)性，當消去順序將導致新的觀測聚集在樹根部分時是比較好的，因為新的測量添加進來時，對數結構破壞程度最小。

總結

總結一下貝葉斯樹是如何處理iSAM1遺留下來的問題：
如何減少重排序的次數，如何降低新增狀態(tài)量，對R矩陣稀疏性的破壞程度

變量消除法實際上是在對雅可比矩陣求QR分解，只不過不需要全局分解，可以通過逐步的局部操作完成，這樣就說明了局部的QR分解不會影響全局的一致性。是后面貝葉斯樹可以局部更新的前提。

貝葉斯樹，通過維護樹結構，當新增狀態(tài)量時，通過樹結構快速找出，受影響的其他變量，將該部分樹結構刪除，并重新線性化生成貝葉斯網再生成貝葉斯樹。而其他未受影響的子樹結構得以保留，從而減少了計算。

同時在重新生成貝葉斯網的過程中，局部的變量順序得到重新排序，從而使未來添加新變量的代價最小（貝葉斯樹改動，以及需要分解的因子圖規(guī)模最小）。舉個例子，如果把iSAM1需要周期性的批處理來得到較優(yōu)的變量消元順序，比作打撲克時，抽完所有牌再理牌的話，那么貝葉斯樹的增量更新過程更像是再每次抓牌的過程中都將局部牌序調整到一個比較優(yōu)的順序，從而長期保持矩陣的稀疏性。但是是否能一直保持下去呢，答案是不能的，當遇到意料外的大回環(huán)，還是會影響很大一部分的樹結構，從而導致大規(guī)模的因子圖重建，排序消元，構建貝葉斯樹。因此減少但并沒有完全解決這個問題。

總結

以上是生活随笔為你收集整理的【论文阅读】iSAM贝叶斯树相关内容理解与学习的全部內容，希望文章能夠幫你解決所遇到的問題。

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