1. 算法流程

?

2. update函數(shù)運(yùn)行流程

2.1. updateDelta: Update delta if we need it to check relinearization later

2.2. update.pushBackFactors: Add any new factors

• 為每一個新的factor產(chǎn)生一個索引，把新factor裝入nonlinearFactors中
• 把需要移除的factor從nonlinearFactors和linearFactors中同時移除，把從nonlinearFactors中移除的factor移入removedFactors
• Remove removed factors from the variable index so we do not attempt to relinearize them
• computeUnusedKeys: 計(jì)算無用的key

Get keys from removed factors and new factors, and compute unused keys, i.e., keys that are empty now and do not appear in the new factors.

• 統(tǒng)計(jì)被移除的因子關(guān)聯(lián)的key，如果這個key不存在于成員變量variableIndex_中，則記錄下來。為什么會不存在于variableIndex_中？
• 統(tǒng)計(jì)新因子關(guān)系的key
• 第一步獲得的集合與第二步獲得的集合取差集，即無用的key的集合，記為unusedKeys

2.3. addVariables: Initialize any new variables

• 把新變量放到成員變量theta_中，初始化為0
• 在detail中標(biāo)記哪些變量是新的

2.4. update.error: Calculate nonlinear error

2.5. gatherInvolvedKeys: 收集相關(guān)key

• 收集新因子和移除的因子相關(guān)的key
• 收集需要重新線性化的因子
• Also, keys that were not observed in existing factors, but whose affected keys have been extended now (e.g. smart factors)
• 把這些key一起返回，作為相關(guān)key，記為markedKeys

2.6. updateKeys

• 遍歷markedKeys
• 在detail中把相關(guān)key標(biāo)記為isObserved
• 如果相關(guān)key不存在于unusedKeys中，也就是說它不是無用key，則把它復(fù)制到已觀測的key中，記為observedKeys
• 收集需要重線性化的變量

2.7. gatherRelinearizeKeys: 標(biāo)記變化量大于閾值的變量，記為relinKeys

在detail中把它們標(biāo)記成isAboveRelinThreshold和isRelinearized

2.8. update.findFluid: Mark cliques that involve marked variables and ancestors.

2.8.1. 從成員變量roots_的元素中遞歸尋找，其元素是團(tuán)

2.8.2. Does the separator contain any of the relinKeys?

2.8.3. If it is true then add this clique to markedKeys

2.8.4. 在detail中進(jìn)行標(biāo)記

2.8.5. UpdateImpl::ExpmapMasked: Update linearization point for marked variables

2.8.6. update.linearizeNewFactors: Linearize new factors

2.8.7. update.augmentVariableIndex:

• Augment the variable index with the new factors
• Augment it with existing factors which now affect to more variables

2.8.8. Recalculate: Redo top of Bayes tree and update data structures

• removeTop: Remove top of Bayes tree and convert to a factor graph: (a) For each affected variable, remove the corresponding clique and all parents up to the root. (b) Store orphaned sub-trees of removed cliques to variable orphans. 對于markedKeys中的每一個key，如果存在于成員變量nodes_中，則該節(jié)點(diǎn)從貝葉斯樹中移除，存入貝葉斯網(wǎng)affectedBayesNet中，生成的孤兒放在orphans中
• 把貝葉斯網(wǎng)affectedBayesNet中的所有key存入affectedKeys中
• recalculateBatch: Do a batch step - reorder and relinearize all variables
• recalculateIncremental: 增量更新，首次update就使用增量更新
• 在detail中標(biāo)記根團(tuán)變量
• 把a(bǔ)ffectedKeysSet中的key存入deltaReplacedMask_中

2.8.9. removeVariables: 移除未使用的變量，所有存在于unusedKeys中的變量都從delta_, theta_等成員變量中移除

2.8.10. update.error: Calculate nonlinear error再做一次

3. recalculateIncremental函數(shù)運(yùn)行流程

3.1. Add the new factors into the resulting factor graph

• 把a(bǔ)ffectedKeys和observedKeys裝入affectedAndNewKeys中
• relinearizeAffectedFactors: 根據(jù)affectedAndNewKeys對nonlinearFactors_中對應(yīng)的因子進(jìn)行重新線性化，返回線性化的得到的線性因子，存入factors中

3.2. detail中對應(yīng)的變量標(biāo)記為isReeliminated

3.3. GetCachedBoundaryFactors: Add the cached intermediate results from the boundary of the orphans，存入factors中

3.3. Add the orphaned subtrees 為啥加兩遍？

3.4. 把markedKeys中的key和affectedKeys中的key放入affectedKeysSet中

3.5. 用因子圖factors初始化VariableIndex類型變量affectedFactorsVarIndex

3.6. Re-order and eliminate the factor graph into a Bayes net (Algorithm [alg:eliminate]), and re-assemble into a new Bayes tree (Algorithm [alg:BayesTree])

• create a partial reordering for the new and contaminated factors result->markedKeys are passed in: those variables will be forced to the end in the ordering, Create ordering constraints，constraintGroups即updateParams.constrainedKeys
• Remove unaffected keys from the constraints，如果某key存在于unusedKeys中，即它未被使用，或者它不存在于affectedKeysSet中，即它未被影響，則把它從constraintGroups中刪除，即不參與重排序
• Ordering::ColamdConstrained 對affectedFactorsVarIndex依據(jù)constraintGroups進(jìn)行重排序

3.7. Do elimination

• 構(gòu)建高斯消元數(shù)GaussianEliminationTree
• 構(gòu)建聯(lián)合樹ISAM2JunctionTree
• 聯(lián)合樹消元，根據(jù)參數(shù)調(diào)用對應(yīng)的消元函數(shù)，消元得到貝葉斯樹和剩余因子圖
• 把貝葉斯樹的根放入成員變量roots_中，把節(jié)點(diǎn)放入成員變量nodes_中

4. EliminatableClusterTree::eliminate函數(shù)運(yùn)行流程

4.1. 構(gòu)造EliminationData::EliminationPostOrderVisitor類，這個構(gòu)造函數(shù)也是比較簡單的，就是用形參來初始化成員變量

4.2. 運(yùn)行treeTraversal::DepthFirstForest函數(shù)，進(jìn)行深度優(yōu)先搜索

• 該函數(shù)不會改變傳入的第一個實(shí)參*this的任何內(nèi)容
• 用EliminatableClusterTree的所有根團(tuán)構(gòu)造TraversalNode對象，并依次入棧，這些TraversalNode對象的父節(jié)點(diǎn)都是EliminationData
• 不斷從棧頂取元素，直到棧為空

4.3. 把所有有根節(jié)點(diǎn)收集起來，放入result->roots_中

4.4. 把所有的remaining收集起來

4.4. 二者組成pair，返回

5. recalculateBatch函數(shù)運(yùn)行流程

6. marginalizeLeaves函數(shù)運(yùn)行流程

這個函數(shù)寫得不太好，一個函數(shù)有太多行；

函數(shù)首先是定義變量，初始化變量，定義一個lambda表達(dá)式等簡單工作，然后才開始正式的工作

6.1. 定義trackingRemoveSubtree，這是一個lambda表達(dá)式，供后面使用

• 輸入為根團(tuán)subtreeRoot，輸出為被移除的團(tuán)removedCliques
• 調(diào)用BayesTree::removeSubtree函數(shù)把subtreeRoot及其后代從nodes_中移除

6.2. 對leafKeys中的每一個key，如果已經(jīng)存在于leafKeysRemoved中了，表示已經(jīng)處理過了，則跳過，沒有處理過的，才進(jìn)行下一步處理

6.3. 通過nodes_找到這個key對應(yīng)的clique

6.4. 不斷地去尋找clique的父節(jié)點(diǎn)（有比較難以理解的判斷條件）并把父節(jié)點(diǎn)賦值給clique，這個判斷條件有待深入理解，為何只用團(tuán)中第一個key去判斷

6.5. 如果clique的frontals()都在leafKeys中，則標(biāo)記為刪除整個團(tuán)

6.6. 如果標(biāo)記為刪除整個團(tuán)，則調(diào)用trackingRemoveSubtree刪除cliquee及其后代，并把邊緣化因子存入到marginalFactors中

6.7. 如果未標(biāo)記為刪除整個團(tuán)，待補(bǔ)充

6.8. At this point we have updated the BayesTree, now update the remaining iSAM2 data structures

6.9. 把邊緣化產(chǎn)生的因子裝進(jìn)nonlinearFactors_，linearFactors_和variableIndex_中

6.10. variableIndex_，delta_，nodes_，theta_刪除無用的變量

參考文獻(xiàn)

iSAM2: Incremental Smoothing and Mapping with Fluid Relinearization and Incremental Variable Reordering

The Bayes Tree: An Algorithmic Foundation for Probabilistic Robot Mapping

總結(jié)

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

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