gtsam  4.1.0
gtsam
ISAM-inst.h
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
18 #pragma once
19 
20 #include <gtsam/inference/ISAM.h>
22 
23 namespace gtsam {
24 
25 /* ************************************************************************* */
26 template<class BAYESTREE>
27 void ISAM<BAYESTREE>::updateInternal(const FactorGraphType& newFactors,
28  Cliques* orphans, const Eliminate& function) {
29  // Remove the contaminated part of the Bayes tree
30  BayesNetType bn;
31  const KeySet newFactorKeys = newFactors.keys();
32  if (!this->empty()) {
33  KeyVector keyVector(newFactorKeys.begin(), newFactorKeys.end());
34  this->removeTop(keyVector, &bn, orphans);
35  }
36 
37  // Add the removed top and the new factors
38  FactorGraphType factors;
39  factors += bn;
40  factors += newFactors;
41 
42  // Add the orphaned subtrees
43  for (const sharedClique& orphan : *orphans)
44  factors += boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan);
45 
46  // Get an ordering where the new keys are eliminated last
47  const VariableIndex index(factors);
48  const Ordering ordering = Ordering::ColamdConstrainedLast(index,
49  KeyVector(newFactorKeys.begin(), newFactorKeys.end()));
50 
51  // eliminate all factors (top, added, orphans) into a new Bayes tree
52  auto bayesTree = factors.eliminateMultifrontal(ordering, function, index);
53 
54  // Re-add into Bayes tree data structures
55  this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(),
56  bayesTree->roots().end());
57  this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end());
58 }
59 
60 /* ************************************************************************* */
61 template<class BAYESTREE>
62 void ISAM<BAYESTREE>::update(const FactorGraphType& newFactors,
63  const Eliminate& function) {
64  Cliques orphans;
65  this->updateInternal(newFactors, &orphans, function);
66 }
67 
68 }
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::Ordering
Definition: Ordering.h:34
VariableIndex.h
gtsam::FastList
Definition: FastList.h:38
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
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:101
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::FastSet< Key >
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
ISAM.h
Incremental update functionality (iSAM) for BayesTree.
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition: VariableIndex.h:43