Go to the documentation of this file.
39 value = conditional->negLogConstant();
41 return {std::dynamic_pointer_cast<GaussianFactor>(conditional),
value};
76 const std::vector<GaussianConditional::shared_ptr> &
conditionals)
95 auto constantFactor = std::make_shared<JacobianFactor>(
c);
108 if (node) total += 1;
117 if (!ptr)
return nullptr;
118 auto conditional = std::dynamic_pointer_cast<GaussianConditional>(ptr);
122 throw std::logic_error(
123 "A HybridGaussianConditional unexpectedly contained a non-conditional");
129 const This *
e =
dynamic_cast<const This *
>(&lf);
130 if (
e ==
nullptr)
return false;
141 return f1->equals(*(f2), tol);
148 std::cout << (
s.empty() ?
"" :
s +
"\n");
151 if (
isHybrid()) std::cout <<
"Hybrid ";
153 std::cout <<
" Discrete Keys = ";
155 std::cout <<
"(" <<
formatter(dk.first) <<
", " << dk.second <<
"), ";
157 std::cout << std::endl
158 <<
" logNormalizationConstant: " << -
negLogConstant() << std::endl
164 if (gf && !gf->empty()) {
180 const Key key = discreteKey.first;
182 continuousParentKeys.erase(std::remove(continuousParentKeys.begin(),
183 continuousParentKeys.end(),
key),
184 continuousParentKeys.end());
186 return continuousParentKeys;
192 for (
auto &&kv : given) {
193 if (given.find(kv.first) == given.end()) {
204 throw std::runtime_error(
205 "HybridGaussianConditional::likelihood: given values are missing some "
215 const auto likelihood_m = conditional->likelihood(given);
216 const double Cgm_Kgcm = conditional->negLogConstant() -
negLogConstant_;
217 if (Cgm_Kgcm == 0.0) {
218 return {likelihood_m, 0.0};
222 return {likelihood_m, Cgm_Kgcm};
225 return std::make_shared<HybridGaussianFactor>(
226 continuousParentKeys, discreteParentKeys, likelihoods);
231 std::set<DiscreteKey>
s(discreteKeys.begin(), discreteKeys.end());
251 auto pruner = [discreteProbs, discreteProbsKeySet, hybridGaussianCondKeySet](
260 if (hybridGaussianCondKeySet == discreteProbsKeySet) {
261 if (discreteProbs(
values) == 0.0) {
263 std::shared_ptr<GaussianConditional>
null;
269 std::vector<DiscreteKey> set_diff;
271 discreteProbsKeySet.begin(), discreteProbsKeySet.end(),
272 hybridGaussianCondKeySet.begin(), hybridGaussianCondKeySet.end(),
273 std::back_inserter(set_diff));
275 const std::vector<DiscreteValues> assignments =
279 augmented_values.
insert(assignment);
283 if (discreteProbs(augmented_values) > 0.0) {
313 return conditional->logProbability(continuousValues);
327 return conditional->logProbability(
values.continuous());
333 return conditional->evaluate(
values.continuous());
void print(const std::string &s="Conditional", const KeyFormatter &formatter=DefaultKeyFormatter) const
Linear Factor Graph where all factors are Gaussians.
A hybrid conditional in the Conditional Linear Gaussian scheme.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Conditionals conditionals_
bool empty() const
Check if tree is empty.
bool equals(const DecisionTree &other, const CompareFunc &compare=&DefaultCompare) const
bool isDiscrete() const
True if this is a factor of discrete variables only.
mxArray * wrap(const Class &value)
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.
Chordal Bayes Net, the result of eliminating a factor graph.
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.
A set of GaussianFactors, indexed by a set of discrete keys.
const std::vector< GaussianConditional::shared_ptr > conditionals
Double_ range(const Point2_ &p, const Point2_ &q)
AlgebraicDecisionTree< Key > logProbability(const VectorValues &continuousValues) const
Compute logProbability of the HybridGaussianConditional as a tree.
double evaluate(const HybridValues &values) const override
Calculate probability density for given values.
NodePtr root_
A DecisionTree just contains the root. TODO(dellaert): make protected.
std::pair< GaussianFactor::shared_ptr, double > GaussianFactorValuePair
Alias for pair of GaussianFactor::shared_pointer and a double value.
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.
size_t nrComponents() const
Returns the total number of continuous components.
std::shared_ptr< This > shared_ptr
shared_ptr to this class
HybridGaussianFactor::FactorValuePairs GetFactorValuePairs(const HybridGaussianConditional::Conditionals &conditionals)
A conditional of gaussian conditionals indexed by discrete variables, as part of a Bayes Network....
void visit(Func f) const
Visit all leaves in depth-first fashion.
KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys)
void prune(const DecisionTreeFactor &discreteProbs)
Prune the decision tree of Gaussian factors as per the discrete discreteProbs.
KeyVector continuousParents() const
Returns the continuous keys among the parents.
bool isContinuous() const
True if this is a factor of continuous variables only.
DecisionTree apply(const Unary &op) const
std::shared_ptr< HybridGaussianFactor > likelihood(const VectorValues &given) const
bool equals(const HybridFactor &lf, double tol=1e-9) const override
equals
std::function< GaussianConditional::shared_ptr(const Assignment< Key > &, const GaussianConditional::shared_ptr &)> prunerFunc(const DecisionTreeFactor &discreteProbs)
Helper function to get the pruner functor.
const gtsam::Symbol key('X', 0)
GaussianFactorGraphTree asGaussianFactorGraphTree() const
Convert a HybridGaussianConditional of conditionals into a DecisionTree of Gaussian factor graphs.
a decision tree is a function from assignments to values.
std::set< DiscreteKey > DiscreteKeysAsSet(const DiscreteKeys &discreteKeys)
Return the DiscreteKey vector as a set.
DecisionTree< Key, GaussianFactorValuePair > FactorValuePairs
typedef for Decision Tree of Gaussian factors and arbitrary value.
GaussianConditional::shared_ptr operator()(const DiscreteValues &discreteValues) const
Return the conditional Gaussian for the given discrete assignment.
std::pair< Key, size_t > DiscreteKey
bool allFrontalsGiven(const VectorValues &given) const
Check whether given has values for all frontal keys.
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.
Implementation of a discrete-conditioned hybrid factor. Implements a joint discrete-continuous factor...
const Conditionals & conditionals() const
Getter for the underlying Conditionals DecisionTree.
std::shared_ptr< This > shared_ptr
shared_ptr to this class
double negLogConstant() const override
Return log normalization constant in negative log space.
HybridGaussianConditional()=default
Default constructor, mainly for serialization.
DiscreteKeys discreteKeys() const
Return all the discrete keys associated with this factor.
std::uint64_t Key
Integer nonlinear key type.
const DiscreteKeys & discreteKeys() const
Return the discrete keys for this factor.
void print(const std::string &s="HybridGaussianConditional\n", const KeyFormatter &formatter=DefaultKeyFormatter) const override
Print utility.
Jet< T, N > sqrt(const Jet< T, N > &f)
std::string str() const
return the string
gtsam
Author(s):
autogenerated on Wed Sep 25 2024 03:02:32