摘要

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

factor grpah

factor graph 結構如下：

在 SLAM 系統中通常假設 Gaussian measurement error 如下圖所示：

SLAM 中的 pose 和 landmark 優化問題可以轉化為圖優化問題，在上圖 factor graph 中：
xi 表示相機的 pose，li 表示 landmark point 坐標，xi 和 li 是 factor graph 中待求解的 variable。

ui 表示測量的相機兩幀之間的位姿關系，mi 表示在相應相機位姿視角下對特征點的測量，p 是預先輸入的 x0 的變量值，在 SLAM 系統中 x0 起到定義坐標系的作用。

SLAM 通常假設 Gaussian measurement model，factor graph 目標函數定義如下：

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

化簡過程參見 iSAM 附錄。

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

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

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

對于公式 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 中變量求解的轉換成，factor graph 從 leaves 到 root 構建 Bayes tree，建立更新的 functions，和 Bayes tree 從 root 到 leaves 變量更新的過程。

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

消去算法如下：

上圖中 (a) 是原始的 factor graph，(b) (c) (d) (e) (f) 分別對應消去變量 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)，當和變量 Δj 相連的變量已知時，可以得到公式 (10)，消去 Δj 得到和 Δj 相關聯的變量之間的聯合概率密度如公式 (11) 所示，同時 factor graph 消去變量還建立了消去的變量的更新公式，如公式 (12) 所示。

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

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

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

Bayes net -> Bayes tree

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

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

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 的結構后，從 root 出發到 leaves 還會逐級向下更新 clique 中的變量的值，相當于根據獲取到的當前幀的信息，更新變量的值。作者視頻中說從 root 向下更新的時候，如果變量值改變的較小，則停止更新，不再向下傳播，但是代碼中說，如果持續的不繼續向下更新，誤差會逐步累積。

同樣更新效果如下圖，更新的時候，如果不存在回環更新只會牽扯到 root 附近的 clique，當存在回環時，牽扯到的 clique 較多，更新起來也相對更復雜。

當有新的 factor 時更新的 Bayes tree 方式如下：

Incrementtal variabl ordering

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

對于 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 端會減少后續 Bayes tree 更新的計算量。作者舉了個例子，如下：

在 (a) 和 (b) 中，新加入一個變量 t6，t2 和 t6 表示存在一個回環，(a) 表示不接受這個回環，如果不接收回環 t6 和 t5 有連接，更新的結果是 t6 掛在葉子節點上；(b) 表示接收回環，如果接收回環 t6 會和 t2、t5 相連接，更新的結果是，把 t6 放到 Bayes tree 的 root 節點上。兩種更新對于 Bayes tree 的 fill in 是等價的，但是 (b) 把變量 t6 放到葉子節點上，使得下次添加和 t6 相關聯的 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

算法如下所示：

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

對于包含需要重新線性化的所有的 clique，和包含變量 clique 的父節點的 cliques（一直到 root clique），都需要根據 factor graph 重新消去變量重新計算變量間的更新關系，作者原話：”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”

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

Partial State Updates

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

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

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

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

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“

參考文獻：
“iSAM2: Incremental Smoothing and Mapping Using the Bayes tree”
“iSAM: Incremental Smoothing and Mappint”

版權聲明：本文為博主原創文章，未經博主允許不得轉載。

總結

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

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