Public Types | Public Member Functions | Protected Types | List of all members
gtsam::HybridGaussianFactorGraph Class Reference

#include <HybridGaussianFactorGraph.h>

Inheritance diagram for gtsam::HybridGaussianFactorGraph:
Inheritance graph
[legend]

Public Types

using Base = HybridFactorGraph
 
using BaseEliminateable = EliminateableFactorGraph< This >
 
using Indices = KeyVector
 map from keys to values More...
 
using shared_ptr = std::shared_ptr< This >
 shared_ptr to This More...
 
using This = HybridGaussianFactorGraph
 for elimination More...
 
using Values = gtsam::Values
 backwards compatibility More...
 
- Public Types inherited from gtsam::HybridFactorGraph
using Base = FactorGraph< Factor >
 
using Indices = KeyVector
 
using shared_ptr = std::shared_ptr< This >
 shared_ptr to This More...
 
using This = HybridFactorGraph
 this class More...
 
using Values = gtsam::Values
 backwards compatibility More...
 
- Public Types inherited from gtsam::FactorGraph< Factor >
typedef FastVector< sharedFactor >::const_iterator const_iterator
 
typedef Factor FactorType
 factor type More...
 
typedef FastVector< sharedFactor >::iterator iterator
 
typedef std::shared_ptr< FactorsharedFactor
 Shared pointer to a factor. More...
 
typedef sharedFactor value_type
 
- Public Types inherited from gtsam::EliminateableFactorGraph< HybridGaussianFactorGraph >
typedef EliminationTraitsType::BayesNetType BayesNetType
 Bayes net type produced by sequential elimination. More...
 
typedef EliminationTraitsType::BayesTreeType BayesTreeType
 Bayes tree type produced by multifrontal elimination. More...
 
typedef EliminationTraitsType::ConditionalType ConditionalType
 Conditional type stored in the Bayes net produced by elimination. More...
 
typedef std::function< EliminationResult(const FactorGraphType &, const Ordering &)> Eliminate
 The function type that does a single dense elimination step on a subgraph. More...
 
typedef std::pair< std::shared_ptr< ConditionalType >, std::shared_ptr< _FactorType > > EliminationResult
 
typedef EliminationTraits< FactorGraphTypeEliminationTraitsType
 Typedef to the specific EliminationTraits for this graph. More...
 
typedef EliminationTraitsType::EliminationTreeType EliminationTreeType
 Elimination tree type that can do sequential elimination of this graph. More...
 
typedef EliminationTraitsType::JunctionTreeType JunctionTreeType
 Junction tree type that can do multifrontal elimination of this graph. More...
 
typedef std::optional< Ordering::OrderingTypeOptionalOrderingType
 Typedef for an optional ordering type. More...
 
typedef std::optional< std::reference_wrapper< const VariableIndex > > OptionalVariableIndex
 

Public Member Functions

GaussianFactorGraph choose (const DiscreteValues &assignment) const
 Get the GaussianFactorGraph at a given discrete assignment. Note this corresponds to the Gaussian posterior p(X|M=m, Z=z) of the continuous variables X given the discrete assignment M=m and whatever measurements z where assumed in the creation of the factor Graph. More...
 
GaussianFactorGraph operator() (const DiscreteValues &assignment) const
 Syntactic sugar for choose. More...
 
Constructors
 HybridGaussianFactorGraph ()=default
 Default constructor. More...
 
template<class CONTAINER >
 HybridGaussianFactorGraph (const CONTAINER &factors)
 
 HybridGaussianFactorGraph (std::initializer_list< sharedFactor > factors)
 
template<class DERIVEDFACTOR >
 HybridGaussianFactorGraph (const FactorGraph< DERIVEDFACTOR > &graph)
 
Testable
void print (const std::string &s="HybridGaussianFactorGraph", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
 Print out graph to std::cout, with optional key formatter. More...
 
void printErrors (const HybridValues &values, const std::string &str="HybridGaussianFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter, const std::function< bool(const Factor *, double, size_t)> &printCondition=[](const Factor *, double, size_t) { return true;}) const
 Print the errors of each factor in the hybrid factor graph. More...
 
Standard Interface
AlgebraicDecisionTree< KeyerrorTree (const VectorValues &continuousValues) const
 Compute error for each discrete assignment, and return as a tree. More...
 
double probPrime (const HybridValues &values) const
 Compute the unnormalized posterior probability for a continuous vector values given a specific assignment. More...
 
AlgebraicDecisionTree< KeydiscretePosterior (const VectorValues &continuousValues) const
 Computer posterior P(M|X=x) when all continuous values X are given. This is efficient as this simply probPrime normalized. More...
 
HybridGaussianProductFactor collectProductFactor () const
 Create a decision tree of factor graphs out of this hybrid factor graph. More...
 
std::pair< std::shared_ptr< HybridConditional >, std::shared_ptr< Factor > > eliminate (const Ordering &keys) const
 Eliminate the given continuous keys. More...
 
- Public Member Functions inherited from gtsam::HybridFactorGraph
 HybridFactorGraph ()=default
 Default constructor. More...
 
template<class DERIVEDFACTOR >
 HybridFactorGraph (const FactorGraph< DERIVEDFACTOR > &graph)
 
template<class CONTAINER >
 HybridFactorGraph (const CONTAINER &factors)
 
std::set< DiscreteKeydiscreteKeys () const
 Get all the discrete keys in the factor graph. More...
 
KeySet discreteKeySet () const
 Get all the discrete keys in the factor graph, as a set of Keys. More...
 
const KeySet continuousKeySet () const
 Get all the continuous keys in the factor graph. More...
 
- Public Member Functions inherited from gtsam::FactorGraph< Factor >
 FactorGraph (std::initializer_list< std::shared_ptr< DERIVEDFACTOR >> sharedFactors)
 
virtual ~FactorGraph ()=default
 
void reserve (size_t size)
 
IsDerived< DERIVEDFACTOR > push_back (std::shared_ptr< DERIVEDFACTOR > factor)
 Add a factor directly using a shared_ptr. More...
 
IsDerived< DERIVEDFACTOR > push_back (const DERIVEDFACTOR &factor)
 
IsDerived< DERIVEDFACTOR > emplace_shared (Args &&... args)
 Emplace a shared pointer to factor of given type. More...
 
IsDerived< DERIVEDFACTOR > add (std::shared_ptr< DERIVEDFACTOR > factor)
 add is a synonym for push_back. More...
 
std::enable_if< std::is_base_of< FactorType, DERIVEDFACTOR >::value, This >::typeoperator+= (std::shared_ptr< DERIVEDFACTOR > factor)
 Append factor to factor graph. More...
 
std::enable_if< std::is_base_of< FactorType, DERIVEDFACTOR >::value, This >::typeoperator, (std::shared_ptr< DERIVEDFACTOR > factor)
 Overload comma operator to allow for append chaining. More...
 
HasDerivedElementType< ITERATOR > push_back (ITERATOR firstFactor, ITERATOR lastFactor)
 
HasDerivedValueType< ITERATOR > push_back (ITERATOR firstFactor, ITERATOR lastFactor)
 Push back many factors with an iterator (factors are copied) More...
 
HasDerivedElementType< CONTAINER > push_back (const CONTAINER &container)
 
HasDerivedValueType< CONTAINER > push_back (const CONTAINER &container)
 Push back non-pointer objects in a container (factors are copied). More...
 
void add (const FACTOR_OR_CONTAINER &factorOrContainer)
 
Thisoperator+= (const FACTOR_OR_CONTAINER &factorOrContainer)
 
std::enable_if< std::is_base_of< This, typename CLIQUE::FactorGraphType >::value >::type push_back (const BayesTree< CLIQUE > &bayesTree)
 
FactorIndices add_factors (const CONTAINER &factors, bool useEmptySlots=false)
 
bool equals (const This &fg, double tol=1e-9) const
 Check equality up to tolerance. More...
 
size_t size () const
 
bool empty () const
 
const sharedFactor at (size_t i) const
 
sharedFactorat (size_t i)
 
std::shared_ptr< F > at (size_t i)
 
const std::shared_ptr< F > at (size_t i) const
 Const version of templated at method. More...
 
const sharedFactor operator[] (size_t i) const
 
sharedFactoroperator[] (size_t i)
 
const_iterator begin () const
 
const_iterator end () const
 
sharedFactor front () const
 
sharedFactor back () const
 
double error (const HybridValues &values) const
 
iterator begin ()
 
iterator end ()
 
virtual void resize (size_t size)
 
void remove (size_t i)
 
void replace (size_t index, sharedFactor factor)
 
iterator erase (iterator item)
 
iterator erase (iterator first, iterator last)
 
void dot (std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
 Output to graphviz format, stream version. More...
 
std::string dot (const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
 Output to graphviz format string. More...
 
void saveGraph (const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
 output to file with graphviz format. More...
 
size_t nrFactors () const
 
KeySet keys () const
 
KeyVector keyVector () const
 
bool exists (size_t idx) const
 
- Public Member Functions inherited from gtsam::EliminateableFactorGraph< HybridGaussianFactorGraph >
std::shared_ptr< BayesTreeTypeeliminateMultifrontal (const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::shared_ptr< BayesTreeTypeeliminateMultifrontal (OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::pair< std::shared_ptr< BayesTreeType >, std::shared_ptr< FactorGraphType > > eliminatePartialMultifrontal (const KeyVector &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::pair< std::shared_ptr< BayesTreeType >, std::shared_ptr< FactorGraphType > > eliminatePartialMultifrontal (const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::pair< std::shared_ptr< BayesNetType >, std::shared_ptr< FactorGraphType > > eliminatePartialSequential (const KeyVector &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::pair< std::shared_ptr< BayesNetType >, std::shared_ptr< FactorGraphType > > eliminatePartialSequential (const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::shared_ptr< BayesNetTypeeliminateSequential (const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::shared_ptr< BayesNetTypeeliminateSequential (OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::shared_ptr< FactorGraphTypemarginal (const KeyVector &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::shared_ptr< BayesNetTypemarginalMultifrontalBayesNet (const KeyVector &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::shared_ptr< BayesNetTypemarginalMultifrontalBayesNet (const KeyVector &variables, const Ordering &marginalizedVariableOrdering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::shared_ptr< BayesNetTypemarginalMultifrontalBayesNet (const Ordering &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::shared_ptr< BayesNetTypemarginalMultifrontalBayesNet (const Ordering &variables, const Ordering &marginalizedVariableOrdering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::shared_ptr< BayesTreeTypemarginalMultifrontalBayesTree (const KeyVector &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::shared_ptr< BayesTreeTypemarginalMultifrontalBayesTree (const KeyVector &variables, const Ordering &marginalizedVariableOrdering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::shared_ptr< BayesTreeTypemarginalMultifrontalBayesTree (const Ordering &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::shared_ptr< BayesTreeTypemarginalMultifrontalBayesTree (const Ordering &variables, const Ordering &marginalizedVariableOrdering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 

Protected Types

template<typename FACTOR >
using IsGaussian = typename std::enable_if< std::is_base_of< GaussianFactor, FACTOR >::value >::type
 Check if FACTOR type is derived from GaussianFactor. More...
 

Additional Inherited Members

- Protected Member Functions inherited from gtsam::FactorGraph< Factor >
bool isEqual (const FactorGraph &other) const
 Check exact equality of the factor pointers. Useful for derived ==. More...
 
 FactorGraph ()
 
 FactorGraph (ITERATOR firstFactor, ITERATOR lastFactor)
 
 FactorGraph (const CONTAINER &factors)
 
- Protected Attributes inherited from gtsam::FactorGraph< Factor >
FastVector< sharedFactorfactors_
 

Detailed Description

Hybrid Gaussian Factor Graph

This is the linearized version of a hybrid factor graph.

Definition at line 105 of file HybridGaussianFactorGraph.h.

Member Typedef Documentation

◆ Base

Definition at line 115 of file HybridGaussianFactorGraph.h.

◆ BaseEliminateable

Definition at line 118 of file HybridGaussianFactorGraph.h.

◆ Indices

map from keys to values

Definition at line 122 of file HybridGaussianFactorGraph.h.

◆ IsGaussian

template<typename FACTOR >
using gtsam::HybridGaussianFactorGraph::IsGaussian = typename std::enable_if< std::is_base_of<GaussianFactor, FACTOR>::value>::type
protected

Check if FACTOR type is derived from GaussianFactor.

Definition at line 112 of file HybridGaussianFactorGraph.h.

◆ shared_ptr

shared_ptr to This

Definition at line 119 of file HybridGaussianFactorGraph.h.

◆ This

for elimination

Definition at line 117 of file HybridGaussianFactorGraph.h.

◆ Values

backwards compatibility

Definition at line 121 of file HybridGaussianFactorGraph.h.

Constructor & Destructor Documentation

◆ HybridGaussianFactorGraph() [1/4]

gtsam::HybridGaussianFactorGraph::HybridGaussianFactorGraph ( )
default

Default constructor.

◆ HybridGaussianFactorGraph() [2/4]

template<class CONTAINER >
gtsam::HybridGaussianFactorGraph::HybridGaussianFactorGraph ( const CONTAINER &  factors)
inlineexplicit

Construct from container of factors (shared_ptr or plain objects)

Definition at line 132 of file HybridGaussianFactorGraph.h.

◆ HybridGaussianFactorGraph() [3/4]

gtsam::HybridGaussianFactorGraph::HybridGaussianFactorGraph ( std::initializer_list< sharedFactor factors)
inline

Construct from an initializer lists of GaussianFactor shared pointers. Example: HybridGaussianFactorGraph graph = { factor1, factor2, factor3 };

Definition at line 140 of file HybridGaussianFactorGraph.h.

◆ HybridGaussianFactorGraph() [4/4]

template<class DERIVEDFACTOR >
gtsam::HybridGaussianFactorGraph::HybridGaussianFactorGraph ( const FactorGraph< DERIVEDFACTOR > &  graph)
inline

Implicit copy/downcast constructor to override explicit template container constructor. In BayesTree this is used for: cachedSeparatorMarginal_.reset(*separatorMarginal)

Definition at line 149 of file HybridGaussianFactorGraph.h.

Member Function Documentation

◆ choose()

GaussianFactorGraph gtsam::HybridGaussianFactorGraph::choose ( const DiscreteValues assignment) const

Get the GaussianFactorGraph at a given discrete assignment. Note this corresponds to the Gaussian posterior p(X|M=m, Z=z) of the continuous variables X given the discrete assignment M=m and whatever measurements z where assumed in the creation of the factor Graph.

Note
Be careful, as any factors not Gaussian are ignored.
Parameters
assignmentThe discrete value assignment for the discrete keys.
Returns
Gaussian factors as a GaussianFactorGraph

Definition at line 559 of file HybridGaussianFactorGraph.cpp.

◆ collectProductFactor()

HybridGaussianProductFactor gtsam::HybridGaussianFactorGraph::collectProductFactor ( ) const

Create a decision tree of factor graphs out of this hybrid factor graph.

For example, if there are two hybrid factors, one with a discrete key A and one with a discrete key B, then the decision tree will have two levels, one for A and one for B. The leaves of the tree will be the Gaussian factors that have only continuous keys.

Definition at line 176 of file HybridGaussianFactorGraph.cpp.

◆ discretePosterior()

AlgebraicDecisionTree< Key > gtsam::HybridGaussianFactorGraph::discretePosterior ( const VectorValues continuousValues) const

Computer posterior P(M|X=x) when all continuous values X are given. This is efficient as this simply probPrime normalized.

Note
Not a DiscreteConditional as the cardinalities of the DiscreteKeys, which we would need, are hard to recover.
Parameters
continuousValuesContinuous values x to condition on.
Returns
DecisionTreeFactor

Definition at line 548 of file HybridGaussianFactorGraph.cpp.

◆ eliminate()

std::pair< HybridConditional::shared_ptr, std::shared_ptr< Factor > > gtsam::HybridGaussianFactorGraph::eliminate ( const Ordering keys) const

Eliminate the given continuous keys.

Parameters
keysThe continuous keys to eliminate.
Returns
The conditional on the keys and a factor on the separator.

<<<<<< MOST COMPUTE IS HERE

Definition at line 357 of file HybridGaussianFactorGraph.cpp.

◆ errorTree()

AlgebraicDecisionTree< Key > gtsam::HybridGaussianFactorGraph::errorTree ( const VectorValues continuousValues) const

Compute error for each discrete assignment, and return as a tree.

Error $ e = \Vert x - \mu \Vert_{\Sigma} $.

Parameters
continuousValuesContinuous values at which to compute the error.
Returns
AlgebraicDecisionTree<Key>

Definition at line 519 of file HybridGaussianFactorGraph.cpp.

◆ operator()()

GaussianFactorGraph gtsam::HybridGaussianFactorGraph::operator() ( const DiscreteValues assignment) const
inline

Syntactic sugar for choose.

Definition at line 254 of file HybridGaussianFactorGraph.h.

◆ print()

void gtsam::HybridGaussianFactorGraph::print ( const std::string &  s = "HybridGaussianFactorGraph",
const KeyFormatter formatter = DefaultKeyFormatter 
) const
overridevirtual

Print out graph to std::cout, with optional key formatter.

Reimplemented from gtsam::FactorGraph< Factor >.

Definition at line 129 of file HybridGaussianFactorGraph.cpp.

◆ printErrors()

void gtsam::HybridGaussianFactorGraph::printErrors ( const HybridValues values,
const std::string &  str = "HybridGaussianFactorGraph: ",
const KeyFormatter keyFormatter = DefaultKeyFormatter,
const std::function< bool(const Factor *, double, size_t)> &  printCondition = [](const Factor*, double, size_t) { return true; } 
) const

Print the errors of each factor in the hybrid factor graph.

Parameters
valuesThe HybridValues for the variables used to compute the error.
strString that is output before the factor graph and errors.
keyFormatterFormatter function for the keys in the factors.
printConditionA condition to check if a factor should be printed.

Definition at line 149 of file HybridGaussianFactorGraph.cpp.

◆ probPrime()

double gtsam::HybridGaussianFactorGraph::probPrime ( const HybridValues values) const

Compute the unnormalized posterior probability for a continuous vector values given a specific assignment.

Returns
double

Definition at line 541 of file HybridGaussianFactorGraph.cpp.


The documentation for this class was generated from the following files:


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:51:33