Incremental Smoothing and Mapping (ISAM) algorithm for hybrid factor graphs. More...
#include <HybridGaussianISAM.h>
Public Member Functions | |
void | update (const HybridGaussianFactorGraph &newFactors, const std::optional< size_t > &maxNrLeaves={}, const std::optional< Ordering > &ordering={}, const HybridBayesTree::Eliminate &function=HybridBayesTree::EliminationTraitsType::DefaultEliminate) |
Perform update step with new factors. More... | |
Standard Constructors | |
HybridGaussianISAM () | |
HybridGaussianISAM (const HybridBayesTree &bayesTree) | |
Public Member Functions inherited from gtsam::ISAM< HybridBayesTree > | |
ISAM () | |
ISAM (const Base &bayesTree) | |
void | update (const FactorGraphType &newFactors, const Eliminate &function=EliminationTraitsType::DefaultEliminate) |
void | updateInternal (const FactorGraphType &newFactors, Cliques *orphans, const Eliminate &function=EliminationTraitsType::DefaultEliminate) |
Public Member Functions inherited from gtsam::HybridBayesTree | |
HybridBayesTree ()=default | |
bool | equals (const This &other, double tol=1e-9) const |
GaussianBayesTree | choose (const DiscreteValues &assignment) const |
Get the Gaussian Bayes Tree which corresponds to a specific discrete value assignment. More... | |
double | error (const HybridValues &values) const |
HybridValues | optimize () const |
Optimize the hybrid Bayes tree by computing the MPE for the current set of discrete variables and using it to compute the best continuous update delta. More... | |
VectorValues | optimize (const DiscreteValues &assignment) const |
Recursively optimize the BayesTree to produce a vector solution. More... | |
void | prune (const size_t maxNumberLeaves) |
Prune the underlying Bayes tree. More... | |
Public Member Functions inherited from gtsam::BayesTree< HybridBayesTreeClique > | |
size_t | size () const |
bool | empty () const |
const Nodes & | nodes () const |
sharedClique | operator[] (Key j) const |
const Roots & | roots () const |
const sharedClique & | clique (Key j) const |
BayesTreeCliqueData | getCliqueData () const |
size_t | numCachedSeparatorMarginals () const |
sharedConditional | marginalFactor (Key j, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const |
sharedFactorGraph | joint (Key j1, Key j2, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const |
sharedBayesNet | jointBayesNet (Key j1, Key j2, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const |
void | dot (std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const |
Output to graphviz format, stream version. More... | |
std::string | dot (const KeyFormatter &keyFormatter=DefaultKeyFormatter) const |
Output to graphviz format string. More... | |
void | saveGraph (const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const |
output to file with graphviz format. More... | |
Key | findParentClique (const CONTAINER &parents) const |
void | clear () |
void | deleteCachedShortcuts () |
void | removePath (sharedClique clique, BayesNetType *bn, Cliques *orphans) |
void | removeTop (const KeyVector &keys, BayesNetType *bn, Cliques *orphans) |
Cliques | removeSubtree (const sharedClique &subtree) |
void | insertRoot (const sharedClique &subtree) |
void | addClique (const sharedClique &clique, const sharedClique &parent_clique=sharedClique()) |
void | addFactorsToGraph (FactorGraph< FactorType > *graph) const |
void | print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const |
Static Public Member Functions | |
static Ordering | GetOrdering (HybridGaussianFactorGraph &factors, const HybridGaussianFactorGraph &newFactors) |
Helper method to get an ordering given the existing factors and any new factors added. More... | |
Private Member Functions | |
void | updateInternal (const HybridGaussianFactorGraph &newFactors, HybridBayesTree::Cliques *orphans, const std::optional< size_t > &maxNrLeaves={}, const std::optional< Ordering > &ordering={}, const HybridBayesTree::Eliminate &function=HybridBayesTree::EliminationTraitsType::DefaultEliminate) |
Internal method that performs the ISAM update. More... | |
Additional Inherited Members | |
Protected Types inherited from gtsam::BayesTree< HybridBayesTreeClique > | |
typedef std::shared_ptr< This > | shared_ptr |
typedef BayesTree< HybridBayesTreeClique > | This |
Protected Member Functions inherited from gtsam::BayesTree< HybridBayesTreeClique > | |
This & | operator= (const This &other) |
~BayesTree () | |
BayesTree () | |
BayesTree (const This &other) | |
void | getCliqueData (sharedClique clique, BayesTreeCliqueData *stats) const |
void | dot (std::ostream &s, sharedClique clique, const KeyFormatter &keyFormatter, int parentnum=0) const |
void | removeClique (sharedClique clique) |
void | fillNodesIndex (const sharedClique &subtree) |
bool | equals (const This &other, double tol=1e-9) const |
Protected Attributes inherited from gtsam::BayesTree< HybridBayesTreeClique > | |
Nodes | nodes_ |
Roots | roots_ |
Incremental Smoothing and Mapping (ISAM) algorithm for hybrid factor graphs.
Definition at line 37 of file HybridGaussianISAM.h.
Definition at line 39 of file HybridGaussianISAM.h.
typedef std::shared_ptr<This> gtsam::HybridGaussianISAM::shared_ptr |
Definition at line 41 of file HybridGaussianISAM.h.
Definition at line 40 of file HybridGaussianISAM.h.
gtsam::HybridGaussianISAM::HybridGaussianISAM | ( | ) |
Create an empty Bayes Tree
Definition at line 35 of file HybridGaussianISAM.cpp.
gtsam::HybridGaussianISAM::HybridGaussianISAM | ( | const HybridBayesTree & | bayesTree | ) |
Copy constructor
Definition at line 38 of file HybridGaussianISAM.cpp.
|
static |
Helper method to get an ordering given the existing factors and any new factors added.
factors | The existing factors in the BayesTree. |
newFactors | New factors added during the update step. |
Definition at line 42 of file HybridGaussianISAM.cpp.
void gtsam::HybridGaussianISAM::update | ( | const HybridGaussianFactorGraph & | newFactors, |
const std::optional< size_t > & | maxNrLeaves = {} , |
||
const std::optional< Ordering > & | ordering = {} , |
||
const HybridBayesTree::Eliminate & | function = HybridBayesTree::EliminationTraitsType::DefaultEliminate |
||
) |
Perform update step with new factors.
newFactors | Factor graph of new factors to add and eliminate. |
maxNrLeaves | The maximum number of leaves to keep after pruning. |
ordering | Custom elimination ordering. |
function | Elimination function. |
Definition at line 117 of file HybridGaussianISAM.cpp.
|
private |
Internal method that performs the ISAM update.
Definition at line 71 of file HybridGaussianISAM.cpp.