Isam2 operation process

1. Algorithm flow

 

 2. update Function running process

1. updateDelta: Update delta if we need it to check relinearization later
2. update.pushBackFactors: Add any new factors
   1. For every new factor Generate an index , Put new factor Load nonlinearFactors in
   2. Remove what needs to be removed factor from nonlinearFactors and linearFactors Remove at the same time , Take from nonlinearFactors Removed from factor move removedFactors
   3. Remove removed factors from the variable index so we do not attempt to relinearize them
3. computeUnusedKeys: Calculation is useless 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. Statistics are associated with removed factors key, If this key Does not exist in member variables variableIndex_ in , Then write it down . Why does it not exist in variableIndex_ in ？
2. Statistics of new factor relationships key
3. The set obtained in the first step is different from the set obtained in the second step , It's useless key Set , Write it down as unusedKeys

1. addVariables: Initialize any new variables
   1. Put the new variable into the member variable theta_ in , Initialize to 0
   2. stay detail Mark which variables are new
2. update.error: Calculate nonlinear error
3. gatherInvolvedKeys: Collect relevant information key
   1. Collect new factors related to removed factors key
   2. Collect the factors that need to be re linearized
   3. Also, keys that were not observed in existing factors, but whose affected keys have been extended now (e.g. smart factors)
   4. Put these key Back together , As related key, Write it down as markedKeys
4. updateKeys:
   1. Traverse markedKeys
   2. stay detail Zhongba correlation key Marked as isObserved
   3. If relevant key There is no in unusedKeys in , That is to say, it is not useless key, Then copy it to the observed key in , Write it down as observedKeys
5. Collect variables that need to be re linearized
   1. gatherRelinearizeKeys: Mark variables whose change is greater than the threshold , Write it down as relinKeys
   2. stay detail Mark them as isAboveRelinThreshold and isRelinearized
6. update.findFluid: Mark cliques that involve marked variables and ancestors.
   1. From member variable roots_ Recursively find , Its element is a group
   2. Does the separator contain any of the relinKeys?
   3. If it is true then add this clique to markedKeys
   4. stay detail Mark in
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. about markedKeys Each of them key, If it exists in the member variable nodes_ in , Then the node is removed from the Bayesian tree , Deposit into Bayesian Network affectedBayesNet in , The orphans are placed in orphans in
    2. Put Bayesian Network affectedBayesNet All in key Deposit in affectedKeys in
    3. recalculateBatch: Do a batch step - reorder and relinearize all variables
    4. recalculateIncremental: Incremental updating , For the first time update Just use incremental update
    5. stay detail Mark the root group variable in
    6. hold affectedKeysSet Medium key Deposit in deltaReplacedMask_ in
11. removeVariables: Remove unused variables , All exist in unusedKeys The variables in are all from delta_, theta_ And other member variables
12. update.error: Calculate nonlinear error Do it again

3. recalculateIncremental Function running process

1. Add the new factors into the resulting factor graph
   1. hold affectedKeys and observedKeys Load affectedAndNewKeys in
   2. relinearizeAffectedFactors: according to affectedAndNewKeys Yes nonlinearFactors_ Re linearize the corresponding factor in , Returns the linearized linear factor , Deposit in factors in
2. detail The corresponding variable in is marked isReeliminated
3. GetCachedBoundaryFactors: Add the cached intermediate results from the boundary of the orphans, Deposit in factors in
4. Add the orphaned subtrees Why add it twice ？
5. hold markedKeys Medium key and affectedKeys Medium key Put in affectedKeysSet in
6. Use factor graph factors initialization VariableIndex Type variable 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 namely updateParams.constrainedKeys
   2. Remove unaffected keys from the constraints, If a key Exist in unusedKeys in , That is, it is not used , Or it doesn't exist affectedKeysSet in , That is, it is not affected , Then take it from constraintGroups Delete in , That is, do not participate in reordering
   3. Ordering::ColamdConstrained Yes affectedFactorsVarIndex basis constraintGroups Reorder
8. Do elimination
   1. Construct Gaussian elimination GaussianEliminationTree
   2. Build a union tree ISAM2JunctionTree
   3. Union tree elimination , Call the corresponding elimination function according to the parameters , Bayes tree and residual factor graph are obtained by elimination
   4. Put the root of Bayesian tree into member variable roots_ in , Put the node into the member variable nodes_ in

4. EliminatableClusterTree::eliminate Function running process

 

1. structure EliminationData::EliminationPostOrderVisitor class , This constructor is also relatively simple , Is to initialize member variables with formal parameters
2. function treeTraversal::DepthFirstForest function , Depth first search
   1. This function does not change the first argument passed *this Any content of
   2. use EliminatableClusterTree All root group structures of TraversalNode object , And put them on the stack in turn , these TraversalNode The parent nodes of objects are EliminationData
   3. Constantly take elements from the top of the stack , Until the stack is empty
3. Collect all the root nodes , Put in result->roots_ in
4. Put all the remaining Gather up
5. The two make up pair, return

5. recalculateBatch Function running process

6. marginalizeLeaves Function running process

This function is not well written , A function has too many lines ;

Functions first define variables , Initialize variable , Define a lambda Expressions and other simple work , Then start the formal work

1. Definition trackingRemoveSubtree, This is a lambda expression , For later use
   1. Input as root group subtreeRoot, Output as removed clique removedCliques
   2. call BayesTree::removeSubtree Function subtreeRoot And their descendants from nodes_ Remove
2. Yes leafKeys Each of them key, If it already exists in leafKeysRemoved It's in , Indicates that it has been processed , Then skip , They haven't been dealt with , Before proceeding to the next step
3. adopt nodes_ To find the key Corresponding clique
4. Keep looking clique Parent node （ There are relatively difficult judgment conditions ） And assign the parent node to clique, This judgment condition needs to be further understood , Why only use the first in the Group key To judge
5. If clique Of frontals() All in leafKeys in , Then it is marked as deleting the whole group
6. If it is marked to delete the whole group , Call trackingRemoveSubtree Delete cliquee And their descendants , And store the marginalization factor into marginalFactors in
7. If it is not marked to delete the whole group , To be added
8. At this point we have updated the BayesTree, now update the remaining iSAM2 data structures
9. Put the factors generated by marginalization into nonlinearFactors_,linearFactors_ and variableIndex_ in
10. variableIndex_,delta_,nodes_,theta_ Delete useless variables