26template<
class BAYESTREE>
28 Cliques* orphans,
const Eliminate& function) {
31 const KeySet newFactorKeys = newFactors.keys();
33 KeyVector keyVector(newFactorKeys.begin(), newFactorKeys.end());
34 this->removeTop(keyVector, &bn, orphans);
38 FactorGraphType factors;
40 factors += newFactors;
43 for (
const sharedClique& orphan : *orphans)
49 KeyVector(newFactorKeys.begin(), newFactorKeys.end()));
52 auto bayesTree = factors.eliminateMultifrontal(ordering, function, index);
55 this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(),
56 bayesTree->roots().end());
57 this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end());
61template<
class BAYESTREE>
63 const Eliminate& function) {
65 this->updateInternal(newFactors, &orphans, function);
Incremental update functionality (iSAM) for BayesTree.
Global functions in a separate testing namespace.
Definition chartTesting.h:28
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
Definition BayesTree.h:276
void update(const FactorGraphType &newFactors, const Eliminate &function=EliminationTraitsType::DefaultEliminate)
update the Bayes tree with a set of new factors, typically derived from measurements
Definition ISAM-inst.h:62
void updateInternal(const FactorGraphType &newFactors, Cliques *orphans, const Eliminate &function=EliminationTraitsType::DefaultEliminate)
updateInternal provides access to list of orphans for drawing purposes
Definition ISAM-inst.h:27
static Ordering ColamdConstrainedLast(const FACTOR_GRAPH &graph, const KeyVector &constrainLast, bool forceOrder=false)
Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details for note o...
Definition Ordering.h:114
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition VariableIndex.h:43