Go to the documentation of this file.
46 this->
logConstant_, conditional->logNormalizationConstant());
60 std::vector<GaussianConditional::shared_ptr> &&
conditionals)
61 :
GaussianMixture(continuousFrontals, continuousParents, discreteParents,
68 const std::vector<GaussianConditional::shared_ptr> &
conditionals)
69 :
GaussianMixture(continuousFrontals, continuousParents, discreteParents,
108 if (!ptr)
return nullptr;
109 auto conditional = std::dynamic_pointer_cast<GaussianConditional>(ptr);
113 throw std::logic_error(
114 "A GaussianMixture unexpectedly contained a non-conditional");
119 const This *
e =
dynamic_cast<const This *
>(&lf);
120 if (
e ==
nullptr)
return false;
131 return f1->equals(*(f2), tol);
138 std::cout << (
s.empty() ?
"" :
s +
"\n");
141 if (
isHybrid()) std::cout <<
"Hybrid ";
143 std::cout <<
" Discrete Keys = ";
145 std::cout <<
"(" <<
formatter(dk.first) <<
", " << dk.second <<
"), ";
152 if (gf && !gf->empty()) {
168 const Key key = discreteKey.first;
170 continuousParentKeys.erase(std::remove(continuousParentKeys.begin(),
171 continuousParentKeys.end(),
key),
172 continuousParentKeys.end());
174 return continuousParentKeys;
179 for (
auto &&kv : given) {
180 if (given.find(kv.first) == given.end()) {
191 throw std::runtime_error(
192 "GaussianMixture::likelihood: given values are missing some frontals.");
199 const auto likelihood_m = conditional->likelihood(given);
200 const double Cgm_Kgcm =
202 if (Cgm_Kgcm == 0.0) {
211 auto constantFactor = std::make_shared<JacobianFactor>(
c);
213 return std::make_shared<JacobianFactor>(gfg);
216 return std::make_shared<GaussianMixtureFactor>(
217 continuousParentKeys, discreteParentKeys, likelihoods);
222 std::set<DiscreteKey>
s;
223 s.insert(discreteKeys.begin(), discreteKeys.end());
243 auto pruner = [discreteProbs, discreteProbsKeySet, gaussianMixtureKeySet](
252 if (gaussianMixtureKeySet == discreteProbsKeySet) {
253 if (discreteProbs(
values) == 0.0) {
255 std::shared_ptr<GaussianConditional>
null;
261 std::vector<DiscreteKey> set_diff;
263 discreteProbsKeySet.begin(), discreteProbsKeySet.end(),
264 gaussianMixtureKeySet.begin(), gaussianMixtureKeySet.end(),
265 std::back_inserter(set_diff));
267 const std::vector<DiscreteValues> assignments =
271 augmented_values.
insert(assignment);
275 if (discreteProbs(augmented_values) > 0.0) {
305 return conditional->logProbability(continuousValues);
319 return conditional->error(continuousValues) +
330 return conditional->error(
values.continuous()) +
337 return conditional->logProbability(
values.continuous());
343 return conditional->evaluate(
values.continuous());
GaussianMixture()=default
Default constructor, mainly for serialization.
void print(const std::string &s="Conditional", const KeyFormatter &formatter=DefaultKeyFormatter) const
Linear Factor Graph where all factors are Gaussians.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
virtual bool equals(const HybridFactor &lf, double tol=1e-9) const
equals
bool empty() const
Check if tree is empty.
double logConstant_
log of the normalization constant.
bool equals(const DecisionTree &other, const CompareFunc &compare=&DefaultCompare) const
bool isDiscrete() const
True if this is a factor of discrete variables only.
A conditional of gaussian mixtures indexed by discrete variables, as part of a Bayes Network....
mxArray * wrap(const Class &value)
GaussianFactorGraphTree asGaussianFactorGraphTree() const
Convert a DecisionTree of factors into a DT of Gaussian FGs.
NonlinearFactorGraph graph2()
const KeyFormatter & formatter
double f2(const Vector2 &x)
DiscreteKeys is a set of keys that can be assembled using the & operator.
void print(const std::string &s, const LabelFormatter &labelFormatter, const ValueFormatter &valueFormatter) const
GTSAM-style print.
bool isHybrid() const
True is this is a Discrete-Continuous factor.
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
KeyVector continuousParents() const
Returns the continuous keys among the parents.
const std::vector< GaussianConditional::shared_ptr > conditionals
Double_ range(const Point2_ &p, const Point2_ &q)
void print(const std::string &s="GaussianMixture\n", const KeyFormatter &formatter=DefaultKeyFormatter) const override
Print utility.
NodePtr root_
A DecisionTree just contains the root. TODO(dellaert): make protected.
const Conditionals & conditionals() const
Getter for the underlying Conditionals DecisionTree.
static std::vector< DiscreteValues > CartesianProduct(const DiscreteKeys &keys)
Return a vector of DiscreteValues, one for each possible combination of values.
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
std::shared_ptr< This > shared_ptr
shared_ptr to this class
std::shared_ptr< GaussianMixtureFactor > likelihood(const VectorValues &given) const
GaussianFactorGraphTree add(const GaussianFactorGraphTree &sum) const
Merge the Gaussian Factor Graphs in this and sum while maintaining the decision tree structure.
void visit(Func f) const
Visit all leaves in depth-first fashion.
A hybrid conditional in the Conditional Linear Gaussian scheme.
KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys)
bool isContinuous() const
True if this is a factor of continuous variables only.
AlgebraicDecisionTree< Key > logProbability(const VectorValues &continuousValues) const
Compute logProbability of the GaussianMixture as a tree.
DecisionTree apply(const Unary &op) const
bool allFrontalsGiven(const VectorValues &given) const
Check whether given has values for all frontal keys.
const gtsam::Symbol key('X', 0)
Conditionals conditionals_
a decision tree of Gaussian conditionals.
std::set< DiscreteKey > DiscreteKeysAsSet(const DiscreteKeys &discreteKeys)
Return the DiscreteKey vector as a set.
void prune(const DecisionTreeFactor &discreteProbs)
Prune the decision tree of Gaussian factors as per the discrete discreteProbs.
AlgebraicDecisionTree< Key > errorTree(const VectorValues &continuousValues) const
Compute error of the GaussianMixture as a tree.
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
std::pair< iterator, bool > insert(const value_type &value)
bool equals(const HybridFactor &lf, double tol=1e-9) const override
Test equality with base HybridFactor.
GaussianConditional::shared_ptr operator()(const DiscreteValues &discreteValues) const
Return the conditional Gaussian for the given discrete assignment.
std::shared_ptr< This > shared_ptr
shared_ptr to this class
size_t nrComponents() const
Returns the total number of continuous components.
DiscreteKeys discreteKeys() const
Return all the discrete keys associated with this factor.
Point2 f1(const Point3 &p, OptionalJacobian< 2, 3 > H)
std::uint64_t Key
Integer nonlinear key type.
double evaluate(const HybridValues &values) const override
Calculate probability density for given values.
std::function< GaussianConditional::shared_ptr(const Assignment< Key > &, const GaussianConditional::shared_ptr &)> prunerFunc(const DecisionTreeFactor &discreteProbs)
Helper function to get the pruner functor.
const DiscreteKeys & discreteKeys() const
Return the discrete keys for this factor.
Jet< T, N > sqrt(const Jet< T, N > &f)
std::string str() const
return the string
A set of GaussianFactors, indexed by a set of discrete keys.
double error(const HybridValues &values) const override
Compute the error of this Gaussian Mixture.
gtsam
Author(s):
autogenerated on Thu Jun 13 2024 03:02:23