ISAM2运行流程

1. 算法流程

2. update函数运行流程

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

2.2. update.pushBackFactors: Add any new factors

• 为每一个新的factor产生一个索引，把新factor装入nonlinearFactors中
• 把需要移除的factor从nonlinearFactors和linearFactors中同时移除，把从nonlinearFactors中移除的factor移入removedFactors
• Remove removed factors from the variable index so we do not attempt to relinearize them
• computeUnusedKeys: 计算无用的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.

• 统计被移除的因子关联的key，如果这个key不存在于成员变量variableIndex_中，则记录下来。为什么会不存在于variableIndex_中？
• 统计新因子关系的key
• 第一步获得的集合与第二步获得的集合取差集，即无用的key的集合，记为unusedKeys

2.3. addVariables: Initialize any new variables

• 把新变量放到成员变量theta_中，初始化为0
• 在detail中标记哪些变量是新的

2.4. update.error: Calculate nonlinear error

2.5. gatherInvolvedKeys: 收集相关key

• 收集新因子和移除的因子相关的key
• 收集需要重新线性化的因子
• Also, keys that were not observed in existing factors, but whose affected keys have been extended now (e.g. smart factors)
• 把这些key一起返回，作为相关key，记为markedKeys

2.6. updateKeys

• 遍历markedKeys
• 在detail中把相关key标记为isObserved
• 如果相关key不存在于unusedKeys中，也就是说它不是无用key，则把它复制到已观测的key中，记为observedKeys
• 收集需要重线性化的变量

2.7. gatherRelinearizeKeys: 标记变化量大于阈值的变量，记为relinKeys

在detail中把它们标记成isAboveRelinThreshold和isRelinearized

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

2.8.1. 从成员变量roots_的元素中递归寻找，其元素是团

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中进行标记

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_中，则该节点从贝叶斯树中移除，存入贝叶斯网affectedBayesNet中，生成的孤儿放在orphans中
• 把贝叶斯网affectedBayesNet中的所有key存入affectedKeys中
• recalculateBatch: Do a batch step - reorder and relinearize all variables
• recalculateIncremental: 增量更新，首次update就使用增量更新
• 在detail中标记根团变量
• 把affectedKeysSet中的key存入deltaReplacedMask_中

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

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

3. recalculateIncremental函数运行流程

3.1. Add the new factors into the resulting factor graph

• 把affectedKeys和observedKeys装入affectedAndNewKeys中
• relinearizeAffectedFactors: 根据affectedAndNewKeys对nonlinearFactors_中对应的因子进行重新线性化，返回线性化的得到的线性因子，存入factors中

3.2. detail中对应的变量标记为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依据constraintGroups进行重排序

3.7. Do elimination

• 构建高斯消元数GaussianEliminationTree
• 构建联合树ISAM2JunctionTree
• 联合树消元，根据参数调用对应的消元函数，消元得到贝叶斯树和剩余因子图
• 把贝叶斯树的根放入成员变量roots_中，把节点放入成员变量nodes_中

4. EliminatableClusterTree::eliminate函数运行流程

4.1. 构造EliminationData::EliminationPostOrderVisitor类，这个构造函数也是比较简单的，就是用形参来初始化成员变量

4.2. 运行treeTraversal::DepthFirstForest函数，进行深度优先搜索

• 该函数不会改变传入的第一个实参*this的任何内容
• 用EliminatableClusterTree的所有根团构造TraversalNode对象，并依次入栈，这些TraversalNode对象的父节点都是EliminationData
• 不断从栈顶取元素，直到栈为空

4.3. 把所有有根节点收集起来，放入result->roots_中

4.4. 把所有的remaining收集起来

4.4. 二者组成pair，返回

5. recalculateBatch函数运行流程

6. marginalizeLeaves函数运行流程

这个函数写得不太好，一个函数有太多行；

函数首先是定义变量，初始化变量，定义一个lambda表达式等简单工作，然后才开始正式的工作

6.1. 定义trackingRemoveSubtree，这是一个lambda表达式，供后面使用

• 输入为根团subtreeRoot，输出为被移除的团removedCliques
• 调用BayesTree::removeSubtree函数把subtreeRoot及其后代从nodes_中移除

6.2. 对leafKeys中的每一个key，如果已经存在于leafKeysRemoved中了，表示已经处理过了，则跳过，没有处理过的，才进行下一步处理

6.3. 通过nodes_找到这个key对应的clique

6.4. 不断地去寻找clique的父节点（有比较难以理解的判断条件）并把父节点赋值给clique，这个判断条件有待深入理解，为何只用团中第一个key去判断

6.5. 如果clique的frontals()都在leafKeys中，则标记为删除整个团

6.6. 如果标记为删除整个团，则调用trackingRemoveSubtree删除cliquee及其后代，并把边缘化因子存入到marginalFactors中

6.7. 如果未标记为删除整个团，待补充

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

6.9. 把边缘化产生的因子装进nonlinearFactors_，linearFactors_和variableIndex_中

6.10. variableIndex_，delta_，nodes_，theta_删除无用的变量

参考文献

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

The Bayes Tree: An Algorithmic Foundation for Probabilistic Robot Mapping