摘要

iSAM2 將圖優(yōu)化問題轉(zhuǎn)換成 Bayes tree 的建立、更新和推理問題。原始待優(yōu)化的問題用 factor graph 表示，factor graph -> Bayes net 建立變量的條件概率，Bayes net -> Bayes tree 建立變量間的更新關(guān)系，Bayes tree 從 leaves 到 root 建立的過程，定義了變量逐級更新的 functions，Bayes tree 從 root 到 leaves 可以逐級更新變量值。iSAM2 整套架構(gòu)建立在概率推理的基礎(chǔ)上，和 iSAM 利用 QR 矩陣分解，當(dāng)有 new factor 時，更新 R 矩陣差別蠻大。

factor grpah

factor graph 結(jié)構(gòu)如下：

在 SLAM 系統(tǒng)中通常假設(shè) Gaussian measurement error 如下圖所示：

SLAM 中的 pose 和 landmark 優(yōu)化問題可以轉(zhuǎn)化為圖優(yōu)化問題，在上圖 factor graph 中：
xi 表示相機的 pose，li 表示 landmark point 坐標(biāo)，xi 和 li 是 factor graph 中待求解的 variable。

ui 表示測量的相機兩幀之間的位姿關(guān)系，mi 表示在相應(yīng)相機位姿視角下對特征點的測量，p 是預(yù)先輸入的 x0 的變量值，在 SLAM 系統(tǒng)中 x0 起到定義坐標(biāo)系的作用。

SLAM 通常假設(shè) Gaussian measurement model，factor graph 目標(biāo)函數(shù)定義如下：

通過對變量線性化化簡上式可得：

化簡過程參見 iSAM 附錄。

上式中的 A 矩陣，和 factor graph 中形式對應(yīng)，如上圖 (a) 所示，在 A 矩陣中，每個 factor 對應(yīng) A 矩陣的一行，factor 中包含到的變量在 A 矩陣中為非 0 元素，A 矩陣的非 0 元素是 factor 對變量一階求導(dǎo)線性化得到。

上圖中 (b) 矩陣 B 對應(yīng) factor graph A 矩陣消元后的結(jié)構(gòu)，factor graph 每次消去一個變量，消去的變量在 square root R 中占據(jù)一行，最后的 R 矩陣的行列數(shù)等于變量的個數(shù)。這里的 R 矩陣只是個圖示，在 iSAM2 實際求解過程中沒有用到 iSAM 算法中 A 矩陣 QR 分解到的 R 矩陣。

根據(jù) factor graph 構(gòu)建的 Bayes tree 形式如 (c) 所示，(c) 中所示的 Bayes tree 和 R 矩陣成對應(yīng)，重申一遍，這里 R 只是拿來做類比，在 iSAM2 求解過程中和 R 矩陣并無關(guān)系。

對于公式 5 的求解可以用 QR 或者 Cholesky 分解算法，流程如下：

factor graph -> Bayes net

作者原話 ”A crucial insight is that inference can be understood as converting the factor graph to a Bayes net using the elimination algorithms“

factor graph 中變量求解的轉(zhuǎn)換成，factor graph 從 leaves 到 root 構(gòu)建 Bayes tree，建立更新的 functions，和 Bayes tree 從 root 到 leaves 變量更新的過程。

factor graph 變量消去過程如下圖所示：

消去算法如下：

上圖中 (a) 是原始的 factor graph，(b) (c) (d) (e) (f) 分別對應(yīng)消去變量 l1 l2 x1 x2 x3 時的 factor graph。

factor graph 消元的過程看 Kaess 視頻和 ppt，Kaess 視頻 youtube 鏈接：
https://www.youtube.com/watch?v=_W3Ua1Yg2fk
視頻 ppt 作者主頁上可以找到。

factor graph 在消元過程中會建立變量的條件概率，例如對于變量 Δj 消元，和變量 Δj 相連的 factor 可以通過化簡可以得到如下公式 (9)，當(dāng)和變量 Δj 相連的變量已知時，可以得到公式 (10)，消去 Δj 得到和 Δj 相關(guān)聯(lián)的變量之間的聯(lián)合概率密度如公式 (11) 所示，同時 factor graph 消去變量還建立了消去的變量的更新公式，如公式 (12) 所示。

通過 factor graph 消元建立了變量的條件概率密度 P(θj|Sj)，其中 θj 是待消去的變量，Sj 是和 θj 在 factor graph 相關(guān)聯(lián)的變量。還同時建立了 Sj 的概率密度 fnew(θj)，和待消去變量的更新關(guān)系（公式12）

后續(xù)會根據(jù) factor grpah 消元產(chǎn)生的 P(θj|Sj) 和 fnew(θj) 建立 Bayes tree，Bayes tree 建立的過程實際是建立了變量的更新過程。

如上原文紅色下劃線描述，求解過程是 Bayes net 轉(zhuǎn)化 Bayes tree，從樹的低端開始到樹的頂端構(gòu)建變量之間的更新的 functions，然后再從樹的頂端開始，根據(jù)公式 (12) 求解變量的最優(yōu)值。

Bayes net -> Bayes tree

從 Bayes net 到 Bayes tree 構(gòu)建過程如下，看Kaess 視頻容易理解。

Bayes tree clique 中條件概率和變量間的更新關(guān)系，是根據(jù)前面 factor graph 到 Bayes net 轉(zhuǎn)換時生成的條件概率和變量更新公式計算得到的，Bayes tree 實際建立了從根節(jié)點到葉子節(jié)點變量的更新關(guān)系。

Incremental inference

作者原文：”When a new measurement is added, for example a factor f′(xj,xj′), only the paths between the cliques containing xj and xj′ and the root are affected. The sub-trees below these cliques are unaffected, as are any other sub-trees not containing xj or xj′. The affected part of the Bayes tree is turned to a factor graph and the new factors are added to it. Using a new elimination order new factors are added to it. “

在添加一個新的 factor 時，例如在 xj 和 xj′ 之間添加一個新的 factor，只是 xj 和 xj′ 之間的 clique 和它們的 ancestor clique 會受影響，受影響的 clique 會在 Bayes tree 中重新拿出來，重新組合，組合完之后再把沒有受到影響的 sub_trees 掛到重新組合后的 clique 下面。

更新完 Bayes tree 的結(jié)構(gòu)后，從 root 出發(fā)到 leaves 還會逐級向下更新 clique 中的變量的值，相當(dāng)于根據(jù)獲取到的當(dāng)前幀的信息，更新變量的值。作者視頻中說從 root 向下更新的時候，如果變量值改變的較小，則停止更新，不再向下傳播，但是代碼中說，如果持續(xù)的不繼續(xù)向下更新，誤差會逐步累積。

同樣更新效果如下圖，更新的時候，如果不存在回環(huán)更新只會牽扯到 root 附近的 clique，當(dāng)存在回環(huán)時，牽扯到的 clique 較多，更新起來也相對更復(fù)雜。

當(dāng)有新的 factor 時更新的 Bayes tree 方式如下：

Incrementtal variabl ordering

iSAM 對矩陣 QR 分解的時候，要有一個好的 variable ordering，一個好的 ordering 會減少矩陣分解的 fill-in，在 Bayes tree 中，fill-in translate to larger clique sizes，導(dǎo)致 clique 中變量個數(shù)太多，這點從變量更新的推導(dǎo)應(yīng)該可以看出來（我自己還未仔細(xì)推導(dǎo)）。

對于 Bayes tree 來說每次更新的時候都可以對 Bayes tree 進行 reorder，”The affected part of the Bayes tree, for which variables have to be reordered, is typically small, as new measurements usually only affect a small subset of the overall state space represented by the variables of the estimation problem.”

和 iSAM 一樣作者也是用 COLAMD 進行 reorder，COLAMD 論文讀起來好吃力，請大神指教。

在 Bayes tree 中的變量進行 reorder 的時候，把新加入的變量放到樹的 root 端會減少后續(xù) Bayes tree 更新的計算量。作者舉了個例子，如下：

在 (a) 和 (b) 中，新加入一個變量 t6，t2 和 t6 表示存在一個回環(huán)，(a) 表示不接受這個回環(huán)，如果不接收回環(huán) t6 和 t5 有連接，更新的結(jié)果是 t6 掛在葉子節(jié)點上；(b) 表示接收回環(huán)，如果接收回環(huán) t6 會和 t2、t5 相連接，更新的結(jié)果是，把 t6 放到 Bayes tree 的 root 節(jié)點上。兩種更新對于 Bayes tree 的 fill in 是等價的，但是 (b) 把變量 t6 放到葉子節(jié)點上，使得下次添加和 t6 相關(guān)聯(lián)的 factor 或者變量時，計算量減少。單純使用 COLAMD reorder 不會考慮到這一點，作者提出使用 constrained COLAMD 進行更新，作者原話：”We use the constrained COLAMD (CCOLAMD) algorithm to both force the most recently accessed variables to the end and still provide a good overall ordering”

The iSAM2 Algorithm

作者原話：”We have already shown how the Bayes tree is updated with new liner factors. That leaves the question of how to deal with nonlinear factors and how to perform this process efficiently by only relinearizing where needed, a process that we call fluid relinearization. To further improve efficiency we restrict the state recovery to the variables that actually change, resulting in partial state updates”
因為在 SLAM 中要解決的是 nonlinear factors，需要對 nonlinear factors 線性化進行求解，作者提出跟蹤實際變化的變量，只對實際變化的變量進行線性化 。

Fluid Relinearization

算法如下所示：

當(dāng)添加新的 factor，Bayes tree 從 root 向 leaves 更新時，會根據(jù)公式 (12)，計算變量更新量，判斷變量是否做更新 Alg. 5：
1、如果對變量的更新量大于一定閾值，對變量 mark
2、根據(jù)計算的更新量 Δj 對變量值進行更新
3、對 clique 中存在 marked variable 的 clique，和它們的 ancestor clique（一直到 root clique）的 clique 做 mark（因為這里需要對變量重新線性化，重新線性化的值，會對所有的 ancestors 的值有影響，Bayes tree 建立了自下到上的依賴關(guān)系）。

對于包含需要重新線性化的所有的 clique，和包含變量 clique 的父節(jié)點的 cliques（一直到 root clique），都需要根據(jù) factor graph 重新消去變量重新計算變量間的更新關(guān)系，作者原話：”Caching those marginal factors during elimination allows restarting of the elimination process from the middle of the tree, rather than having to re-eliminate the complete system”

保存上一次消元時的中間結(jié)果，只需要從最下面一個受影響需要更新的 clique 開始重新線性化就好，不需要對整個 Bayes tree 都做線性化。

Partial State Updates

如上作者指出每次添加新的 factor 時，對于 Bayes tree 中變量不需要逐個都更新，只需要更新那些真的 change 的變量的就可以，可以想象在重建一個有多個房間的大的建筑物時，在一個房間中測量的結(jié)果，不會影響到其它幾個房間中的變量，作者用 Partial state update 的方式，減少更新的變量的個數(shù)，算法如下：

算法 7 中要注意的一點如下圖紅色下劃線所示：

這里改變量是指“更新量的改變量“，相當(dāng)于二階導(dǎo)數(shù)，因為上文中提出，當(dāng)變量的更新量 Δ 超過一定閾值時，對變量重新線性化，也就說 Δ 值可能很大，所以這里判斷變量更新的條件是如果 Δ 的改變量超過一定閾值時，才會對變量做更新。

變量更新是一個不斷向下傳播的過程，在 Bayes tree 中如果當(dāng)前級變量判斷不需要做更新了，當(dāng)前級之后的變量也不需要做更新了。

Algorithm and Complexity

算法的整體框架 Alg. 8 所示，”The goal of our algorithm is to obtain an estimate θ for the variables(map and trajectory), given a set of nonlinear factors. New factors can arrive at any time and may add new variables to the estimation problem. Based on the current linearization point we solve a linearized systemas a subroutine in an iterative nonlinear optimization scheme. The linearized system is represented by the Bayes tree“

參考文獻(xiàn)：
“iSAM2: Incremental Smoothing and Mapping Using the Bayes tree”
“iSAM: Incremental Smoothing and Mappint”

版權(quán)聲明：本文為博主原創(chuàng)文章，未經(jīng)博主允許不得轉(zhuǎn)載。

總結(jié)

以上是生活随笔為你收集整理的iSAM2 笔记的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

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