#include <HybridGaussianFactorGraph.h>
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< Factor > | sharedFactor |
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< FactorGraphType > | EliminationTraitsType |
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::OrderingType > | OptionalOrderingType |
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... | |
DiscreteFactorGraph | discreteFactors () const |
Helper method to get all the discrete factors as a DiscreteFactorGraph. 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< Key > | errorTree (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< Key > | 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. 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< DiscreteKey > | discreteKeys () 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 >::type & | operator+= (std::shared_ptr< DERIVEDFACTOR > factor) |
Append factor to factor graph. More... | |
std::enable_if< std::is_base_of< FactorType, DERIVEDFACTOR >::value, This >::type & | operator, (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) |
This & | operator+= (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 |
sharedFactor & | at (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 |
sharedFactor & | operator[] (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< BayesTreeType > | eliminateMultifrontal (const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const |
std::shared_ptr< BayesTreeType > | eliminateMultifrontal (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< BayesNetType > | eliminateSequential (const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const |
std::shared_ptr< BayesNetType > | eliminateSequential (OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const |
std::shared_ptr< FactorGraphType > | marginal (const KeyVector &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const |
std::shared_ptr< BayesNetType > | marginalMultifrontalBayesNet (const KeyVector &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const |
std::shared_ptr< BayesNetType > | marginalMultifrontalBayesNet (const KeyVector &variables, const Ordering &marginalizedVariableOrdering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const |
std::shared_ptr< BayesNetType > | marginalMultifrontalBayesNet (const Ordering &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const |
std::shared_ptr< BayesNetType > | marginalMultifrontalBayesNet (const Ordering &variables, const Ordering &marginalizedVariableOrdering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const |
std::shared_ptr< BayesTreeType > | marginalMultifrontalBayesTree (const KeyVector &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const |
std::shared_ptr< BayesTreeType > | marginalMultifrontalBayesTree (const KeyVector &variables, const Ordering &marginalizedVariableOrdering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const |
std::shared_ptr< BayesTreeType > | marginalMultifrontalBayesTree (const Ordering &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const |
std::shared_ptr< BayesTreeType > | marginalMultifrontalBayesTree (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< sharedFactor > | factors_ |
This is the linearized version of a hybrid factor graph.
Definition at line 106 of file HybridGaussianFactorGraph.h.
Definition at line 116 of file HybridGaussianFactorGraph.h.
Definition at line 119 of file HybridGaussianFactorGraph.h.
map from keys to values
Definition at line 123 of file HybridGaussianFactorGraph.h.
|
protected |
Check if FACTOR type is derived from GaussianFactor.
Definition at line 113 of file HybridGaussianFactorGraph.h.
using gtsam::HybridGaussianFactorGraph::shared_ptr = std::shared_ptr<This> |
shared_ptr to This
Definition at line 120 of file HybridGaussianFactorGraph.h.
for elimination
Definition at line 118 of file HybridGaussianFactorGraph.h.
backwards compatibility
Definition at line 122 of file HybridGaussianFactorGraph.h.
|
default |
Default constructor.
|
inlineexplicit |
Construct from container of factors (shared_ptr or plain objects)
Definition at line 133 of file HybridGaussianFactorGraph.h.
|
inline |
Construct from an initializer lists of GaussianFactor shared pointers. Example: HybridGaussianFactorGraph graph = { factor1, factor2, factor3 };
Definition at line 141 of file HybridGaussianFactorGraph.h.
|
inline |
Implicit copy/downcast constructor to override explicit template container constructor. In BayesTree this is used for: cachedSeparatorMarginal_.reset(*separatorMarginal)
Definition at line 150 of file HybridGaussianFactorGraph.h.
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.
assignment | The discrete value assignment for the discrete keys. |
Definition at line 560 of file HybridGaussianFactorGraph.cpp.
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 177 of file HybridGaussianFactorGraph.cpp.
DiscreteFactorGraph gtsam::HybridGaussianFactorGraph::discreteFactors | ( | ) | const |
Helper method to get all the discrete factors as a DiscreteFactorGraph.
Definition at line 585 of file HybridGaussianFactorGraph.cpp.
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.
continuousValues | Continuous values x to condition on. |
Definition at line 549 of file HybridGaussianFactorGraph.cpp.
std::pair< HybridConditional::shared_ptr, std::shared_ptr< Factor > > gtsam::HybridGaussianFactorGraph::eliminate | ( | const Ordering & | keys | ) | const |
Eliminate the given continuous keys.
keys | The continuous keys to eliminate. |
<<<<<< MOST COMPUTE IS HERE
Definition at line 358 of file HybridGaussianFactorGraph.cpp.
AlgebraicDecisionTree< Key > gtsam::HybridGaussianFactorGraph::errorTree | ( | const VectorValues & | continuousValues | ) | const |
Compute error for each discrete assignment, and return as a tree.
Error .
continuousValues | Continuous values at which to compute the error. |
Definition at line 520 of file HybridGaussianFactorGraph.cpp.
|
inline |
Syntactic sugar for choose.
Definition at line 255 of file HybridGaussianFactorGraph.h.
|
overridevirtual |
Print out graph to std::cout, with optional key formatter.
Reimplemented from gtsam::FactorGraph< Factor >.
Definition at line 130 of file HybridGaussianFactorGraph.cpp.
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.
values | The HybridValues for the variables used to compute the error. |
str | String that is output before the factor graph and errors. |
keyFormatter | Formatter function for the keys in the factors. |
printCondition | A condition to check if a factor should be printed. |
Definition at line 150 of file HybridGaussianFactorGraph.cpp.
double gtsam::HybridGaussianFactorGraph::probPrime | ( | const HybridValues & | values | ) | const |
Compute the unnormalized posterior probability for a continuous vector values given a specific assignment.
Definition at line 542 of file HybridGaussianFactorGraph.cpp.