A conditional of gaussian conditionals indexed by discrete variables, as part of a Bayes Network. This is the result of the elimination of a continuous variable in a hybrid scheme, such that the remaining variables are discrete+continuous. More...
#include <HybridGaussianConditional.h>
Classes | |
struct | Helper |
Helper struct for constructing HybridGaussianConditional objects. More... | |
Public Types | |
using | BaseConditional = Conditional< BaseFactor, HybridGaussianConditional > |
using | BaseFactor = HybridGaussianFactor |
using | Conditionals = DecisionTree< Key, GaussianConditional::shared_ptr > |
typedef for Decision Tree of Gaussian Conditionals More... | |
using | shared_ptr = std::shared_ptr< This > |
using | This = HybridGaussianConditional |
Public Types inherited from gtsam::HybridGaussianFactor | |
using | Base = HybridFactor |
using | FactorValuePairs = DecisionTree< Key, GaussianFactorValuePair > |
typedef for Decision Tree of Gaussian factors and arbitrary value. More... | |
using | shared_ptr = std::shared_ptr< This > |
using | sharedFactor = std::shared_ptr< GaussianFactor > |
using | This = HybridGaussianFactor |
Public Types inherited from gtsam::HybridFactor | |
typedef Factor | Base |
Our base class. More... | |
enum | Category { Category::None, Category::Discrete, Category::Continuous, Category::Hybrid } |
Enum to help with categorizing hybrid factors. 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 Types inherited from gtsam::Conditional< HybridGaussianFactor, HybridGaussianConditional > | |
typedef std::pair< typename HybridGaussianFactor ::const_iterator, typename HybridGaussianFactor ::const_iterator > | ConstFactorRange |
typedef ConstFactorRangeIterator | Frontals |
typedef ConstFactorRangeIterator | Parents |
Public Member Functions | |
Constructors | |
HybridGaussianConditional ()=default | |
Default constructor, mainly for serialization. More... | |
HybridGaussianConditional (const DiscreteKey &discreteParent, const std::vector< GaussianConditional::shared_ptr > &conditionals) | |
Construct from one discrete key and vector of conditionals. More... | |
HybridGaussianConditional (const DiscreteKey &discreteParent, Key key, const std::vector< std::pair< Vector, double >> ¶meters) | |
Constructs a HybridGaussianConditional with means mu_i and standard deviations sigma_i. More... | |
HybridGaussianConditional (const DiscreteKey &discreteParent, Key key, const Matrix &A, Key parent, const std::vector< std::pair< Vector, double >> ¶meters) | |
Constructs a HybridGaussianConditional with conditional means A × parent + b_i and standard deviations sigma_i. More... | |
HybridGaussianConditional (const DiscreteKey &discreteParent, Key key, const Matrix &A1, Key parent1, const Matrix &A2, Key parent2, const std::vector< std::pair< Vector, double >> ¶meters) | |
Constructs a HybridGaussianConditional with conditional means A1 × parent1 + A2 × parent2 + b_i and standard deviations sigma_i. More... | |
HybridGaussianConditional (const DiscreteKeys &discreteParents, const Conditionals &conditionals) | |
Construct from multiple discrete keys and conditional tree. More... | |
HybridGaussianConditional (const DiscreteKeys &discreteParents, const FactorValuePairs &pairs) | |
Construct from multiple discrete keys M and a tree of factor/scalar pairs, where the scalar is assumed to be the the negative log constant for each assignment m, up to a constant. More... | |
Testable | |
bool | equals (const HybridFactor &lf, double tol=1e-9) const override |
Test equality with base HybridFactor. More... | |
void | print (const std::string &s="HybridGaussianConditional\n", const KeyFormatter &formatter=DefaultKeyFormatter) const override |
Print utility. More... | |
Standard API | |
GaussianConditional::shared_ptr | choose (const DiscreteValues &discreteValues) const |
Return the conditional Gaussian for the given discrete assignment. More... | |
GaussianConditional::shared_ptr | operator() (const DiscreteValues &discreteValues) const |
Syntactic sugar for choose. More... | |
size_t | nrComponents () const |
Returns the total number of continuous components. More... | |
KeyVector | continuousParents () const |
Returns the continuous keys among the parents. More... | |
double | negLogConstant () const override |
Return log normalization constant in negative log space. More... | |
std::shared_ptr< HybridGaussianFactor > | likelihood (const VectorValues &given) const |
const Conditionals | conditionals () const |
double | logProbability (const HybridValues &values) const override |
Compute the logProbability of this hybrid Gaussian conditional. More... | |
double | evaluate (const HybridValues &values) const override |
Calculate probability density for given values . More... | |
double | operator() (const HybridValues &values) const |
Evaluate probability density, sugar. More... | |
HybridGaussianConditional::shared_ptr | prune (const DecisionTreeFactor &discreteProbs) const |
Prune the decision tree of Gaussian factors as per the discrete discreteProbs . More... | |
Public Member Functions inherited from gtsam::HybridGaussianFactor | |
HybridGaussianFactor ()=default | |
Default constructor, mainly for serialization. More... | |
HybridGaussianFactor (const DiscreteKey &discreteKey, const std::vector< GaussianFactor::shared_ptr > &factors) | |
Construct a new HybridGaussianFactor on a single discrete key, providing the factors for each mode m as a vector of factors ϕ_m(x). The value ϕ(x,m) for the factor is simply ϕ_m(x). More... | |
HybridGaussianFactor (const DiscreteKey &discreteKey, const std::vector< GaussianFactorValuePair > &factorPairs) | |
Construct a new HybridGaussianFactor on a single discrete key, including a scalar error value for each mode m. The factors and scalars are provided as a vector of pairs (ϕ_m(x), E_m). The value ϕ(x,m) for the factor is now ϕ_m(x) + E_m. More... | |
HybridGaussianFactor (const DiscreteKeys &discreteKeys, const FactorValuePairs &factorPairs) | |
Construct a new HybridGaussianFactor on a several discrete keys M, including a scalar error value for each assignment m. The factors and scalars are provided as a DecisionTree<Key> of pairs (ϕ_M(x), E_M). The value ϕ(x,M) for the factor is again ϕ_m(x) + E_m. More... | |
bool | equals (const HybridFactor &lf, double tol=1e-9) const override |
equals More... | |
void | print (const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override |
print More... | |
GaussianFactorValuePair | operator() (const DiscreteValues &assignment) const |
Get factor at a given discrete assignment. More... | |
AlgebraicDecisionTree< Key > | errorTree (const VectorValues &continuousValues) const override |
Compute error of the HybridGaussianFactor as a tree. More... | |
double | error (const HybridValues &values) const override |
Compute the log-likelihood, including the log-normalizing constant. More... | |
const FactorValuePairs & | factors () const |
Getter for GaussianFactor decision tree. More... | |
virtual HybridGaussianProductFactor | asProductFactor () const |
Helper function to return factors and functional to create a DecisionTree of Gaussian Factor Graphs. 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... | |
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 () |
Public Member Functions inherited from gtsam::Conditional< HybridGaussianFactor, HybridGaussianConditional > | |
void | print (const std::string &s="Conditional", const KeyFormatter &formatter=DefaultKeyFormatter) const |
bool | equals (const This &c, double tol=1e-9) const |
virtual | ~Conditional () |
size_t | nrFrontals () const |
size_t | nrParents () const |
Key | firstFrontalKey () const |
Frontals | frontals () const |
Parents | parents () const |
double | operator() (const HybridValues &x) const |
Evaluate probability density, sugar. More... | |
virtual double | negLogConstant () const |
All conditional types need to implement this as the negative log of the normalization constant to make it such that error>=0. More... | |
size_t & | nrFrontals () |
HybridGaussianFactor ::const_iterator | beginFrontals () const |
HybridGaussianFactor ::iterator | beginFrontals () |
HybridGaussianFactor ::const_iterator | endFrontals () const |
HybridGaussianFactor ::iterator | endFrontals () |
HybridGaussianFactor ::const_iterator | beginParents () const |
HybridGaussianFactor ::iterator | beginParents () |
HybridGaussianFactor ::const_iterator | endParents () const |
HybridGaussianFactor ::iterator | endParents () |
Private Member Functions | |
bool | allFrontalsGiven (const VectorValues &given) const |
Check whether given has values for all frontal keys. More... | |
HybridGaussianConditional (const DiscreteKeys &discreteParents, Helper &&helper) | |
Private constructor that uses helper struct above. More... | |
Private Attributes | |
double | negLogConstant_ |
Additional Inherited Members | |
Static Public Member Functions inherited from gtsam::Conditional< HybridGaussianFactor, HybridGaussianConditional > | |
static bool | CheckInvariants (const HybridGaussianConditional &conditional, const VALUES &x) |
Protected Member Functions inherited from gtsam::Factor | |
Factor () | |
template<typename CONTAINER > | |
Factor (const CONTAINER &keys) | |
template<typename ITERATOR > | |
Factor (ITERATOR first, ITERATOR last) | |
Protected Member Functions inherited from gtsam::Conditional< HybridGaussianFactor, HybridGaussianConditional > | |
Conditional () | |
Conditional (size_t nrFrontals) | |
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... | |
Protected Attributes inherited from gtsam::Conditional< HybridGaussianFactor, HybridGaussianConditional > | |
size_t | nrFrontals_ |
A conditional of gaussian conditionals indexed by discrete variables, as part of a Bayes Network. This is the result of the elimination of a continuous variable in a hybrid scheme, such that the remaining variables are discrete+continuous.
Represents the conditional density P(X | M, Z) where X is the set of continuous random variables, M is the selection of discrete variables corresponding to a subset of the Gaussian variables and Z is parent of this node .
The probability P(x|y,z,...) is proportional to where i indexes the components and k_i is a component-wise normalization constant.
a density over continuous variables given discrete/continuous parents.
Definition at line 54 of file HybridGaussianConditional.h.
using gtsam::HybridGaussianConditional::BaseConditional = Conditional<BaseFactor, HybridGaussianConditional> |
Definition at line 61 of file HybridGaussianConditional.h.
Definition at line 60 of file HybridGaussianConditional.h.
using gtsam::HybridGaussianConditional::Conditionals = DecisionTree<Key, GaussianConditional::shared_ptr> |
typedef for Decision Tree of Gaussian Conditionals
Definition at line 64 of file HybridGaussianConditional.h.
using gtsam::HybridGaussianConditional::shared_ptr = std::shared_ptr<This> |
Definition at line 59 of file HybridGaussianConditional.h.
Definition at line 58 of file HybridGaussianConditional.h.
|
default |
Default constructor, mainly for serialization.
gtsam::HybridGaussianConditional::HybridGaussianConditional | ( | const DiscreteKey & | discreteParent, |
const std::vector< GaussianConditional::shared_ptr > & | conditionals | ||
) |
Construct from one discrete key and vector of conditionals.
discreteParent | Single discrete parent variable |
conditionals | Vector of conditionals with the same size as the cardinality of the discrete parent. |
Definition at line 135 of file HybridGaussianConditional.cpp.
gtsam::HybridGaussianConditional::HybridGaussianConditional | ( | const DiscreteKey & | discreteParent, |
Key | key, | ||
const std::vector< std::pair< Vector, double >> & | parameters | ||
) |
Constructs a HybridGaussianConditional with means mu_i and standard deviations sigma_i.
discreteParent | The discrete parent or "mode" key. |
key | The key for this conditional variable. |
parameters | A vector of pairs (mu_i, sigma_i). |
Definition at line 141 of file HybridGaussianConditional.cpp.
gtsam::HybridGaussianConditional::HybridGaussianConditional | ( | const DiscreteKey & | discreteParent, |
Key | key, | ||
const Matrix & | A, | ||
Key | parent, | ||
const std::vector< std::pair< Vector, double >> & | parameters | ||
) |
Constructs a HybridGaussianConditional with conditional means A × parent + b_i and standard deviations sigma_i.
discreteParent | The discrete parent or "mode" key. |
key | The key for this conditional variable. |
A | The matrix A. |
parent | The key of the parent variable. |
parameters | A vector of pairs (b_i, sigma_i). |
Definition at line 147 of file HybridGaussianConditional.cpp.
gtsam::HybridGaussianConditional::HybridGaussianConditional | ( | const DiscreteKey & | discreteParent, |
Key | key, | ||
const Matrix & | A1, | ||
Key | parent1, | ||
const Matrix & | A2, | ||
Key | parent2, | ||
const std::vector< std::pair< Vector, double >> & | parameters | ||
) |
Constructs a HybridGaussianConditional with conditional means A1 × parent1 + A2 × parent2 + b_i and standard deviations sigma_i.
discreteParent | The discrete parent or "mode" key. |
key | The key for this conditional variable. |
A1 | The first matrix. |
parent1 | The key of the first parent variable. |
A2 | The second matrix. |
parent2 | The key of the second parent variable. |
parameters | A vector of pairs (b_i, sigma_i). |
Definition at line 155 of file HybridGaussianConditional.cpp.
gtsam::HybridGaussianConditional::HybridGaussianConditional | ( | const DiscreteKeys & | discreteParents, |
const Conditionals & | conditionals | ||
) |
Construct from multiple discrete keys and conditional tree.
discreteParents | the discrete parents. Will be placed last. |
conditionals | a decision tree of GaussianConditionals. The number of conditionals should be C^(number of discrete parents), where C is the cardinality of the DiscreteKeys in discreteParents, since the discreteParents will be used as the labels in the decision tree. |
Definition at line 163 of file HybridGaussianConditional.cpp.
gtsam::HybridGaussianConditional::HybridGaussianConditional | ( | const DiscreteKeys & | discreteParents, |
const FactorValuePairs & | pairs | ||
) |
Construct from multiple discrete keys M and a tree of factor/scalar pairs, where the scalar is assumed to be the the negative log constant for each assignment m, up to a constant.
discreteParents | the discrete parents. Will be placed last. |
conditionalPairs | Decision tree of GaussianFactor/scalar pairs. |
Definition at line 168 of file HybridGaussianConditional.cpp.
|
private |
Private constructor that uses helper struct above.
Definition at line 122 of file HybridGaussianConditional.cpp.
|
private |
Check whether given
has values for all frontal keys.
Definition at line 260 of file HybridGaussianConditional.cpp.
GaussianConditional::shared_ptr gtsam::HybridGaussianConditional::choose | ( | const DiscreteValues & | discreteValues | ) | const |
Return the conditional Gaussian for the given discrete assignment.
Definition at line 190 of file HybridGaussianConditional.cpp.
const HybridGaussianConditional::Conditionals gtsam::HybridGaussianConditional::conditionals | ( | ) | const |
Get Conditionals DecisionTree (dynamic cast from factors)
Definition at line 174 of file HybridGaussianConditional.cpp.
KeyVector gtsam::HybridGaussianConditional::continuousParents | ( | ) | const |
Returns the continuous keys among the parents.
Definition at line 244 of file HybridGaussianConditional.cpp.
|
overridevirtual |
Test equality with base HybridFactor.
Reimplemented from gtsam::HybridFactor.
Definition at line 200 of file HybridGaussianConditional.cpp.
|
overridevirtual |
Calculate probability density for given values
.
Reimplemented from gtsam::Conditional< HybridGaussianFactor, HybridGaussianConditional >.
Definition at line 343 of file HybridGaussianConditional.cpp.
std::shared_ptr< HybridGaussianFactor > gtsam::HybridGaussianConditional::likelihood | ( | const VectorValues & | given | ) | const |
Create a likelihood factor for a hybrid Gaussian conditional, return a hybrid Gaussian factor on the parents.
Definition at line 271 of file HybridGaussianConditional.cpp.
|
overridevirtual |
Compute the logProbability of this hybrid Gaussian conditional.
values | Continuous values and discrete assignment. |
Reimplemented from gtsam::Conditional< HybridGaussianFactor, HybridGaussianConditional >.
Definition at line 335 of file HybridGaussianConditional.cpp.
|
inlineoverride |
Return log normalization constant in negative log space.
The log normalization constant is the min of the individual log-normalization constants.
Definition at line 197 of file HybridGaussianConditional.h.
size_t gtsam::HybridGaussianConditional::nrComponents | ( | ) | const |
Returns the total number of continuous components.
Definition at line 181 of file HybridGaussianConditional.cpp.
|
inline |
Syntactic sugar for choose.
Definition at line 178 of file HybridGaussianConditional.h.
|
inline |
Evaluate probability density, sugar.
Definition at line 222 of file HybridGaussianConditional.h.
|
overridevirtual |
Print utility.
Reimplemented from gtsam::HybridFactor.
Definition at line 218 of file HybridGaussianConditional.cpp.
HybridGaussianConditional::shared_ptr gtsam::HybridGaussianConditional::prune | ( | const DecisionTreeFactor & | discreteProbs | ) | const |
Prune the decision tree of Gaussian factors as per the discrete discreteProbs
.
discreteProbs | A pruned set of probabilities for the discrete keys. |
Definition at line 304 of file HybridGaussianConditional.cpp.
|
private |
< Negative-log of the normalization constant (log(\sqrt(|2πΣ|))). Take advantage of the neg-log space so everything is a minimization
Definition at line 69 of file HybridGaussianConditional.h.