大家好，又见面了，我是你们的朋友全栈君。
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
版权声明：本文内容由互联网用户自发贡献，该文观点仅代表作者本人。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容， 请发送邮件至 举报，一经查实，本站将立刻删除。
发布者：全栈程序员-用户IM，转载请注明出处：https://javaforall.cn/213600.html原文链接：https://javaforall.cn
