Loading...
Searching...
No Matches
ISAM-inst.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/inference/ISAM.h>
#include <gtsam/inference/VariableIndex.h>
namespace gtsam {
/* ************************************************************************* */
template<class BAYESTREE>
void ISAM<BAYESTREE>::updateInternal(const FactorGraphType& newFactors,
Cliques* orphans, const Eliminate& function) {
// Remove the contaminated part of the Bayes tree
BayesNetType bn;
const KeySet newFactorKeys = newFactors.keys();
if (!this->empty()) {
KeyVector keyVector(newFactorKeys.begin(), newFactorKeys.end());
this->removeTop(keyVector, &bn, orphans);
}
// Add the removed top and the new factors
FactorGraphType factors;
factors += bn;
factors += newFactors;
// Add the orphaned subtrees
for (const sharedClique& orphan : *orphans)
factors += boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan);
// Get an ordering where the new keys are eliminated last
const VariableIndex index(factors);
const Ordering ordering = Ordering::ColamdConstrainedLast(index,
KeyVector(newFactorKeys.begin(), newFactorKeys.end()));
// eliminate all factors (top, added, orphans) into a new Bayes tree
auto bayesTree = factors.eliminateMultifrontal(ordering, function, index);
// Re-add into Bayes tree data structures
this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(),
bayesTree->roots().end());
this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end());
}
/* ************************************************************************* */
template<class BAYESTREE>
void ISAM<BAYESTREE>::update(const FactorGraphType& newFactors,
const Eliminate& function) {
Cliques orphans;
this->updateInternal(newFactors, &orphans, function);
}
}
ISAM.h
Incremental update functionality (iSAM) for BayesTree.
VariableIndex.h
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
gtsam::ISAM::update
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
gtsam::ISAM::updateInternal
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
gtsam::Ordering
Definition Ordering.h:34
gtsam::Ordering::ColamdConstrainedLast
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
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition VariableIndex.h:43