Implementation of a discrete conditional mixture factor. Implements a joint discrete-continuous factor where the discrete variable serves to "select" a mixture component corresponding to a GaussianFactor type of measurement. More...
#include <GaussianMixtureFactor.h>
Public Types | |
using | Base = HybridFactor |
using | Factors = DecisionTree< Key, sharedFactor > |
typedef for Decision Tree of Gaussian factors and log-constant. More... | |
using | shared_ptr = std::shared_ptr< This > |
using | sharedFactor = std::shared_ptr< GaussianFactor > |
using | This = GaussianMixtureFactor |
Public Types inherited from gtsam::HybridFactor | |
typedef Factor | Base |
Our base class. More... | |
typedef std::shared_ptr< HybridFactor > | shared_ptr |
shared_ptr to this class More... | |
typedef HybridFactor | This |
This class. More... | |
Public Types inherited from gtsam::Factor | |
typedef KeyVector::const_iterator | const_iterator |
Const iterator over keys. More... | |
typedef KeyVector::iterator | iterator |
Iterator over keys. More... | |
Public Member Functions | |
Constructors | |
GaussianMixtureFactor ()=default | |
Default constructor, mainly for serialization. More... | |
GaussianMixtureFactor (const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, const Factors &factors) | |
Construct a new Gaussian mixture factor. More... | |
GaussianMixtureFactor (const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, const std::vector< sharedFactor > &factors) | |
Construct a new GaussianMixtureFactor object using a vector of GaussianFactor shared pointers. More... | |
Testable | |
bool | equals (const HybridFactor &lf, double tol=1e-9) const override |
equals More... | |
void | print (const std::string &s="GaussianMixtureFactor\, const KeyFormatter &formatter=DefaultKeyFormatter) const override |
print More... | |
Public Member Functions inherited from gtsam::HybridFactor | |
HybridFactor ()=default | |
HybridFactor (const KeyVector &keys) | |
Construct hybrid factor from continuous keys. More... | |
HybridFactor (const DiscreteKeys &discreteKeys) | |
Construct hybrid factor from discrete keys. More... | |
HybridFactor (const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys) | |
Construct a new Hybrid Factor object. More... | |
void | print (const std::string &s="HybridFactor\, const KeyFormatter &formatter=DefaultKeyFormatter) const override |
print More... | |
bool | isDiscrete () const |
True if this is a factor of discrete variables only. More... | |
bool | isContinuous () const |
True if this is a factor of continuous variables only. More... | |
bool | isHybrid () const |
True is this is a Discrete-Continuous factor. More... | |
size_t | nrContinuous () const |
Return the number of continuous variables in this factor. More... | |
const DiscreteKeys & | discreteKeys () const |
Return the discrete keys for this factor. More... | |
const KeyVector & | continuousKeys () const |
Return only the continuous keys for this factor. More... | |
Public Member Functions inherited from gtsam::Factor | |
virtual | ~Factor ()=default |
Default destructor. More... | |
bool | empty () const |
Whether the factor is empty (involves zero variables). More... | |
Key | front () const |
First key. More... | |
Key | back () const |
Last key. More... | |
const_iterator | find (Key key) const |
find More... | |
const KeyVector & | keys () const |
Access the factor's involved variable keys. More... | |
const_iterator | begin () const |
const_iterator | end () const |
size_t | size () const |
virtual void | printKeys (const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const |
print only keys More... | |
bool | equals (const This &other, double tol=1e-9) const |
check equality More... | |
KeyVector & | keys () |
iterator | begin () |
iterator | end () |
Private Member Functions | |
GaussianFactorGraphTree | asGaussianFactorGraphTree () const |
Helper function to return factors and functional to create a DecisionTree of Gaussian Factor Graphs. More... | |
Private Attributes | |
Factors | factors_ |
Decision tree of Gaussian factors indexed by discrete keys. More... | |
Standard API | |
sharedFactor | operator() (const DiscreteValues &assignment) const |
Get factor at a given discrete assignment. More... | |
GaussianFactorGraphTree | add (const GaussianFactorGraphTree &sum) const |
Combine the Gaussian Factor Graphs in sum and this while maintaining the original tree structure. More... | |
AlgebraicDecisionTree< Key > | error (const VectorValues &continuousValues) const |
Compute error of the GaussianMixtureFactor as a tree. More... | |
double | error (const HybridValues &values) const override |
Compute the log-likelihood, including the log-normalizing constant. More... | |
const Factors & | factors () const |
Getter for GaussianFactor decision tree. More... | |
GaussianFactorGraphTree & | operator+= (GaussianFactorGraphTree &sum, const GaussianMixtureFactor &factor) |
Add MixtureFactor to a Sum, syntactic sugar. More... | |
Additional Inherited Members | |
Protected Member Functions inherited from gtsam::Factor | |
Factor () | |
template<typename CONTAINER > | |
Factor (const CONTAINER &keys) | |
template<typename ITERATOR > | |
Factor (ITERATOR first, ITERATOR last) | |
Static Protected Member Functions inherited from gtsam::Factor | |
template<typename CONTAINER > | |
static Factor | FromKeys (const CONTAINER &keys) |
template<typename ITERATOR > | |
static Factor | FromIterators (ITERATOR first, ITERATOR last) |
Protected Attributes inherited from gtsam::HybridFactor | |
KeyVector | continuousKeys_ |
Record continuous keys for book-keeping. More... | |
DiscreteKeys | discreteKeys_ |
Protected Attributes inherited from gtsam::Factor | |
KeyVector | keys_ |
The keys involved in this factor. More... | |
Implementation of a discrete conditional mixture factor. Implements a joint discrete-continuous factor where the discrete variable serves to "select" a mixture component corresponding to a GaussianFactor type of measurement.
Represents the underlying Gaussian mixture as a Decision Tree, where the set of discrete variables indexes to the continuous gaussian distribution.
Definition at line 47 of file GaussianMixtureFactor.h.
Definition at line 49 of file GaussianMixtureFactor.h.
typedef for Decision Tree of Gaussian factors and log-constant.
Definition at line 56 of file GaussianMixtureFactor.h.
using gtsam::GaussianMixtureFactor::shared_ptr = std::shared_ptr<This> |
Definition at line 51 of file GaussianMixtureFactor.h.
using gtsam::GaussianMixtureFactor::sharedFactor = std::shared_ptr<GaussianFactor> |
Definition at line 53 of file GaussianMixtureFactor.h.
Definition at line 50 of file GaussianMixtureFactor.h.
|
default |
Default constructor, mainly for serialization.
gtsam::GaussianMixtureFactor::GaussianMixtureFactor | ( | const KeyVector & | continuousKeys, |
const DiscreteKeys & | discreteKeys, | ||
const Factors & | factors | ||
) |
Construct a new Gaussian mixture factor.
continuousKeys | A vector of keys representing continuous variables. |
discreteKeys | A vector of keys representing discrete variables and their cardinalities. |
factors | The decision tree of Gaussian factors stored as the mixture density. |
Definition at line 32 of file GaussianMixtureFactor.cpp.
|
inline |
Construct a new GaussianMixtureFactor object using a vector of GaussianFactor shared pointers.
continuousKeys | Vector of keys for continuous factors. |
discreteKeys | Vector of discrete keys. |
factors | Vector of gaussian factor shared pointers. |
Definition at line 98 of file GaussianMixtureFactor.h.
GaussianFactorGraphTree gtsam::GaussianMixtureFactor::add | ( | const GaussianFactorGraphTree & | sum | ) | const |
Combine the Gaussian Factor Graphs in sum
and this
while maintaining the original tree structure.
sum | Decision Tree of Gaussian Factor Graphs indexed by the variables. |
Definition at line 85 of file GaussianMixtureFactor.cpp.
|
private |
Helper function to return factors and functional to create a DecisionTree of Gaussian Factor Graphs.
Definition at line 98 of file GaussianMixtureFactor.cpp.
|
overridevirtual |
equals
Reimplemented from gtsam::HybridFactor.
Definition at line 38 of file GaussianMixtureFactor.cpp.
AlgebraicDecisionTree< Key > gtsam::GaussianMixtureFactor::error | ( | const VectorValues & | continuousValues | ) | const |
Compute error of the GaussianMixtureFactor as a tree.
continuousValues | The continuous VectorValues. |
Definition at line 105 of file GaussianMixtureFactor.cpp.
|
overridevirtual |
Compute the log-likelihood, including the log-normalizing constant.
Reimplemented from gtsam::Factor.
Definition at line 116 of file GaussianMixtureFactor.cpp.
|
inline |
Getter for GaussianFactor decision tree.
Definition at line 147 of file GaussianMixtureFactor.h.
GaussianMixtureFactor::sharedFactor gtsam::GaussianMixtureFactor::operator() | ( | const DiscreteValues & | assignment | ) | const |
Get factor at a given discrete assignment.
Definition at line 79 of file GaussianMixtureFactor.cpp.
|
overridevirtual |
|
friend |
Add MixtureFactor to a Sum, syntactic sugar.
Definition at line 150 of file GaussianMixtureFactor.h.
|
private |
Decision tree of Gaussian factors indexed by discrete keys.
Definition at line 60 of file GaussianMixtureFactor.h.