目錄

• iSAM
• • 主要參考論文
  • 因子圖
  • 問(wèn)題轉(zhuǎn)化
  • 增量式因子圖構(gòu)建與問(wèn)題求解
• iSAM2
• • 主要參考論文
  • 一些鋪墊
  • 貝葉斯樹(shù)
• 總結(jié)

iSAM

主要參考論文

論文名iSAM: Incremental Smoothing and Mapping
作者M(jìn)ichael Kaess, Student Member, IEEE, Ananth Ranganathan, Student Member, IEEE,and Frank Dellaert, Member, IEEE
期刊： 2008 T-RO

因子圖

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

問(wèn)題轉(zhuǎn)化

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

$$\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}$$

將問(wèn)題的目標(biāo)函數(shù)在初始的參數(shù)位置進(jìn)行線(xiàn)性化得到如下表達(dá)式：

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

這樣就將非線(xiàn)性最小二乘問(wèn)題轉(zhuǎn)化成了線(xiàn)性最小二乘問(wèn)題，注意到其中的：

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

m為測(cè)量行 n為變量數(shù)，對(duì)于因子圖問(wèn)題每多一個(gè)新的狀態(tài)量一定會(huì)多至少一個(gè)因子，因此m總是大于n的。A是長(zhǎng)條形的，因此對(duì)A進(jìn)行QR分解得到:

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

最小二乘問(wèn)題化簡(jiǎn)成：

$$\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}$$

后半部分為常數(shù)部分，前半部分為二次型，最小值為零，因此最小二乘問(wèn)題被轉(zhuǎn)換成上式前半部分的線(xiàn)性方程組的解。

增量式因子圖構(gòu)建與問(wèn)題求解

如上理論推導(dǎo)是為非線(xiàn)性最小二乘的共性問(wèn)題，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]$$

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

時(shí)只需要對(duì)R矩陣進(jìn)行Givens旋轉(zhuǎn)變換就可以將下面的非零項(xiàng)消除，得到新的R矩陣:

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

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

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

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

因此需要周期性的重排序，來(lái)恢復(fù)矩陣的稀疏性。這一過(guò)程則極為耗時(shí)。

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

iSAM2

主要參考論文

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

一些鋪墊

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

定義一下文章中說(shuō)到的術(shù)語(yǔ)：
消元/消去：代表由因子圖轉(zhuǎn)換為貝葉斯網(wǎng)絡(luò)的過(guò)程中一步操作，將因子圖中的一個(gè)節(jié)點(diǎn)和相連的因子邊轉(zhuǎn)換為貝葉斯網(wǎng)絡(luò)中的節(jié)點(diǎn)和有向邊的過(guò)程。

首先我認(rèn)為貝葉斯樹(shù)本身并沒(méi)有特別的意義，真正的價(jià)值在于你維護(hù)樹(shù)的過(guò)程，當(dāng)你按照貝葉斯樹(shù)的法則去維護(hù)這些變量和狀態(tài)的關(guān)系時(shí)，你會(huì)驚訝的發(fā)現(xiàn)你每次求解問(wèn)題的規(guī)模都是小的，求解的矩陣都是稀疏的，最終你的問(wèn)題過(guò)程都是容易解決的。就像求解方程組，再好的算法都很難從時(shí)間復(fù)雜度上簡(jiǎn)化求解方程組。但是好的算法你會(huì)發(fā)現(xiàn)每一步求解都是十分easy的。或者像堆排序算法，關(guān)鍵在于你維護(hù)這個(gè)堆的過(guò)程。

因子圖定義和前面的文章一樣并沒(méi)有變化。但是貝葉斯樹(shù)是基于貝葉斯網(wǎng)絡(luò)演變過(guò)來(lái)的，貝葉斯網(wǎng)絡(luò)可以參考貝葉斯網(wǎng)專(zhuān)題2：貝葉斯網(wǎng)基本概念_吳智深的博客-CSDN博客.這里面有很細(xì)致的介紹。

許多概率問(wèn)題直接使用聯(lián)合分布進(jìn)行不確定性推理的復(fù)雜度很高。因此，聯(lián)合分布的復(fù)雜度與變量個(gè)數(shù)呈指數(shù)增長(zhǎng)。當(dāng)變量很多時(shí)，聯(lián)合概率的獲取、存儲(chǔ)和運(yùn)算都變得十分困難。

如果通過(guò)連規(guī)則將聯(lián)合概率分布分解為條件概率乘積的形式，再根據(jù)變量之間的獨(dú)立性進(jìn)行化簡(jiǎn)，問(wèn)題就會(huì)簡(jiǎn)化很多。最后將變量之間的依賴(lài)關(guān)系用有向箭頭表示就得到了貝葉斯網(wǎng)。
$$\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}$$

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

因子圖可以轉(zhuǎn)換成貝葉斯網(wǎng)絡(luò)。因子圖問(wèn)題中其實(shí)沒(méi)有因果關(guān)系，但是依然可以表達(dá)成條件概率乘以邊緣概率的形式，形如下式。
$$f_{\text{joint?}}\left({\theta }_j,S_j\right)=P\left({\theta }_j\mid S_j\right)f_{\text{new?}}\left(S_j\right)$$
邊緣概率可能描述的不準(zhǔn)確，因?yàn)榉纸庵皇菍?duì)部分因子進(jìn)行操作，并不代表已經(jīng)包含了所有關(guān)于邊緣某變量的因子，因此、分解得到的條件變量的因子$f_{\text{new?}}\left(S_j\right)$并不等于最終該變量的邊緣概率分布。所以這個(gè)$f_{\text{new?}}\left(S_j\right)$暫時(shí)并不具備實(shí)際意義，只是一個(gè)形如高斯分布的函數(shù)而已。

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

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

這個(gè)過(guò)程和QR分解是一致的，我們先從外觀上對(duì)比一下。


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

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

思考一下為什么一樣，這就要看文中所提出來(lái)的因子分解方法。也就是說(shuō)消元的過(guò)程中實(shí)際上對(duì)因子函數(shù)做了什么。

待分解因子$$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\}$$為與待消除變量相連的全部因子的連乘獲得的，前面已經(jīng)介紹過(guò)。

先給出結(jié)論分解出的條件概率，邊緣因子解析表達(dá)式如下：
$$\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$
其中第二項(xiàng)是通過(guò)將第一項(xiàng)得到的變量增量的最優(yōu)解析表達(dá)${\Delta }_j=d?{\mathbf{rs}}_j$回代回因子中得到的余項(xiàng)。也就是要傳遞給貝葉斯網(wǎng)絡(luò)中父節(jié)點(diǎn)的邊緣因子。（對(duì)應(yīng)圖視角下的相連的狀態(tài)或變量之間兩兩添加一條因子邊的過(guò)程。）

原文中只是給出了解析表達(dá)并沒(méi)有附上詳細(xì)的證明，筆者嘗試證明了一下：
待求最小值的二次型為${\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}$為補(bǔ)全的單位正交基

上式左乘一個(gè)單位正交矩陣不影響其二范數(shù)取值，那么就左乘剛剛提取出來(lái)的正交矩陣的轉(zhuǎn)置。
$$\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}$$
左邊是與當(dāng)前待求參數(shù)相關(guān)的數(shù)值平方項(xiàng)，右面是與當(dāng)前待求參數(shù)無(wú)關(guān)的二范數(shù)項(xiàng)，分別對(duì)應(yīng)條件概率密度（未歸一化）和邊緣因子。與作者所描述分解方法相符。

從上述推導(dǎo)可以看出，由因子圖轉(zhuǎn)換成貝葉斯網(wǎng)絡(luò)的過(guò)程中，每一步實(shí)際在做的是一步QR分解，宏觀上消元順序?qū)?yīng)QR分解順序。

到此就基本解釋了圖視角和矩陣變換視角的對(duì)應(yīng)關(guān)系，以及其對(duì)應(yīng)的概率分解運(yùn)算步驟。

貝葉斯樹(shù)

首先建議先看一下團(tuán)樹(shù)，因?yàn)樽髡叻磸?fù)提到貝葉斯樹(shù)是與團(tuán)樹(shù)或者聯(lián)結(jié)樹(shù)由很高的相似性并且從中發(fā)展來(lái)的。貝葉斯網(wǎng)專(zhuān)題6：團(tuán)樹(shù)傳播_吳智深的博客-CSDN博客_團(tuán)樹(shù)傳播算法.

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

團(tuán)樹(shù)的優(yōu)勢(shì)之一在于推理的復(fù)用，舉個(gè)例子一個(gè)概率模型中包含甲乙丙丁四個(gè)變量，我推理完甲的條件概率，當(dāng)我需要推理乙的條件概率時(shí)，可以復(fù)用甲推理過(guò)程中的部分中間推理結(jié)果。使用團(tuán)樹(shù)可以很清晰的維護(hù)這些中間結(jié)果，方便信息傳遞以及推理復(fù)用。

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

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

當(dāng)新增測(cè)量因子時(shí)，比如當(dāng)$x_2,x_3$之間產(chǎn)生了共視關(guān)系。那么含有$x_2$的團(tuán)$l_1,x_1:x_2$

及其父團(tuán)都將受到影響，如圖所示圈中的團(tuán)受到影響。而綠色的子樹(shù)部分沒(méi)有受到影響。

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

這里說(shuō)明了貝葉斯樹(shù)是更好的維持了與線(xiàn)性代數(shù)的等價(jià)性，并使算法支持遞歸估計(jì)，可并行計(jì)算。

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

我們注意到當(dāng)變量的增量值已經(jīng)很大時(shí)，是需要重線(xiàn)性化的，文中使用閾值來(lái)判斷哪些變量需要重線(xiàn)性化。同時(shí)使用另一個(gè)閾值來(lái)判斷，重新線(xiàn)性化后或者添加了新的變量或者測(cè)量之后，第二步，從上到下計(jì)算增量，的過(guò)程是否需要重新計(jì)算（因?yàn)闂l件變量的取值變了，但是如果增量變化很小的話(huà)，就可以停止這個(gè)計(jì)算過(guò)程。下面的子樹(shù)中的增量就也不需要更新了）。這兩步屬于加速算法的trick，利用的是，slam問(wèn)題新的觀測(cè)往往對(duì)較遠(yuǎn)的狀態(tài)估計(jì)產(chǎn)生很小的影響。

此外就是每次貝葉斯樹(shù)的生成過(guò)程中，消元順序的選擇，這里會(huì)最終影響貝葉斯樹(shù)的結(jié)構(gòu)。求最優(yōu)消元順序?qū)嶋H上是NP難的問(wèn)題，一般使用啟發(fā)式的方法求解。

文中介紹了一種啟發(fā)性，當(dāng)消去順序?qū)?dǎo)致新的觀測(cè)聚集在樹(shù)根部分時(shí)是比較好的，因?yàn)樾碌臏y(cè)量添加進(jìn)來(lái)時(shí)，對(duì)數(shù)結(jié)構(gòu)破壞程度最小。

總結(jié)

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

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

貝葉斯樹(shù)，通過(guò)維護(hù)樹(shù)結(jié)構(gòu)，當(dāng)新增狀態(tài)量時(shí)，通過(guò)樹(shù)結(jié)構(gòu)快速找出，受影響的其他變量，將該部分樹(shù)結(jié)構(gòu)刪除，并重新線(xiàn)性化生成貝葉斯網(wǎng)再生成貝葉斯樹(shù)。而其他未受影響的子樹(shù)結(jié)構(gòu)得以保留，從而減少了計(jì)算。

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

總結(jié)

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

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