isam2运行流程

1. 算法流程

 

 2. update函数运行流程

1. updateDelta: Update delta if we need it to check relinearization later
2. update.pushBackFactors: Add any new factors
   1. 为每一个新的factor产生一个索引，把新factor装入nonlinearFactors中
   2. 把需要移除的factor从nonlinearFactors和linearFactors中同时移除，把从nonlinearFactors中移除的factor移入removedFactors
   3. Remove removed factors from the variable index so we do not attempt to relinearize them
3. 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.

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

1. addVariables: Initialize any new variables
   1. 把新变量放到成员变量theta_中，初始化为0
   2. 在detail中标记哪些变量是新的
2. update.error: Calculate nonlinear error
3. gatherInvolvedKeys: 收集相关key
   1. 收集新因子和移除的因子相关的key
   2. 收集需要重新线性化的因子
   3. Also, keys that were not observed in existing factors, but whose affected keys have been extended now (e.g. smart factors)
   4. 把这些key一起返回，作为相关key，记为markedKeys
4. updateKeys:
   1. 遍历markedKeys
   2. 在detail中把相关key标记为isObserved
   3. 如果相关key不存在于unusedKeys中，也就是说它不是无用key，则把它复制到已观测的key中，记为observedKeys
5. 收集需要重线性化的变量
   1. gatherRelinearizeKeys: 标记变化量大于阈值的变量，记为relinKeys
   2. 在detail中把它们标记成isAboveRelinThreshold和isRelinearized
6. update.findFluid: Mark cliques that involve marked variables and ancestors.
   1. 从成员变量roots_的元素中递归寻找，其元素是团
   2. Does the separator contain any of the relinKeys?
   3. If it is true then add this clique to markedKeys
   4. 在detail中进行标记
7. UpdateImpl::ExpmapMasked: Update linearization point for marked variables
8. update.linearizeNewFactors: Linearize new factors
9. update.augmentVariableIndex:
   1. Augment the variable index with the new factors
   2. Augment it with existing factors which now affect to more variables
10. Recalculate: Redo top of Bayes tree and update data structures
    1. 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中
    2. 把贝叶斯网affectedBayesNet中的所有key存入affectedKeys中
    3. recalculateBatch: Do a batch step - reorder and relinearize all variables
    4. recalculateIncremental: 增量更新，首次update就使用增量更新
    5. 在detail中标记根团变量
    6. 把affectedKeysSet中的key存入deltaReplacedMask_中
11. removeVariables: 移除未使用的变量，所有存在于unusedKeys中的变量都从delta_, theta_等成员变量中移除
12. update.error: Calculate nonlinear error再做一次

3. recalculateIncremental函数运行流程

1. Add the new factors into the resulting factor graph
   1. 把affectedKeys和observedKeys装入affectedAndNewKeys中
   2. relinearizeAffectedFactors: 根据affectedAndNewKeys对nonlinearFactors_中对应的因子进行重新线性化，返回线性化的得到的线性因子，存入factors中
2. detail中对应的变量标记为isReeliminated
3. GetCachedBoundaryFactors: Add the cached intermediate results from the boundary of the orphans，存入factors中
4. Add the orphaned subtrees 为啥加两遍？
5. 把markedKeys中的key和affectedKeys中的key放入affectedKeysSet中
6. 用因子图factors初始化VariableIndex类型变量affectedFactorsVarIndex
7. 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])
   1. 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
   2. Remove unaffected keys from the constraints，如果某key存在于unusedKeys中，即它未被使用，或者它不存在于affectedKeysSet中，即它未被影响，则把它从constraintGroups中删除，即不参与重排序
   3. Ordering::ColamdConstrained 对affectedFactorsVarIndex依据constraintGroups进行重排序
8. Do elimination
   1. 构建高斯消元数GaussianEliminationTree
   2. 构建联合树ISAM2JunctionTree
   3. 联合树消元，根据参数调用对应的消元函数，消元得到贝叶斯树和剩余因子图
   4. 把贝叶斯树的根放入成员变量roots_中，把节点放入成员变量nodes_中

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

 

1. 构造EliminationData::EliminationPostOrderVisitor类，这个构造函数也是比较简单的，就是用形参来初始化成员变量
2. 运行treeTraversal::DepthFirstForest函数，进行深度优先搜索
   1. 该函数不会改变传入的第一个实参*this的任何内容
   2. 用EliminatableClusterTree的所有根团构造TraversalNode对象，并依次入栈，这些TraversalNode对象的父节点都是EliminationData
   3. 不断从栈顶取元素，直到栈为空
3. 把所有有根节点收集起来，放入result->roots_中
4. 把所有的remaining收集起来
5. 二者组成pair，返回

5. recalculateBatch函数运行流程

6. marginalizeLeaves函数运行流程

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

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

1. 定义trackingRemoveSubtree，这是一个lambda表达式，供后面使用
   1. 输入为根团subtreeRoot，输出为被移除的团removedCliques
   2. 调用BayesTree::removeSubtree函数把subtreeRoot及其后代从nodes_中移除
2. 对leafKeys中的每一个key，如果已经存在于leafKeysRemoved中了，表示已经处理过了，则跳过，没有处理过的，才进行下一步处理
3. 通过nodes_找到这个key对应的clique
4. 不断地去寻找clique的父节点（有比较难以理解的判断条件）并把父节点赋值给clique，这个判断条件有待深入理解，为何只用团中第一个key去判断
5. 如果clique的frontals()都在leafKeys中，则标记为删除整个团
6. 如果标记为删除整个团，则调用trackingRemoveSubtree删除cliquee及其后代，并把边缘化因子存入到marginalFactors中
7. 如果未标记为删除整个团，待补充
8. At this point we have updated the BayesTree, now update the remaining iSAM2 data structures
9. 把边缘化产生的因子装进nonlinearFactors_，linearFactors_和variableIndex_中
10. variableIndex_，delta_，nodes_，theta_删除无用的变量