Go to the documentation of this file.
49 value =
c->negLogConstant();
52 return {std::dynamic_pointer_cast<GaussianFactor>(
c),
value};
56 throw std::runtime_error(
57 "HybridGaussianConditional: need at least one frontal variable.");
70 negLogConstant_(helper.minNegLogConstant) {}
80 const std::vector<GaussianConditional::shared_ptr> &
conditionals)
104 auto constantFactor = std::make_shared<JacobianFactor>(
c);
117 if (node) total += 1;
126 if (!ptr)
return nullptr;
127 auto conditional = std::dynamic_pointer_cast<GaussianConditional>(ptr);
131 throw std::logic_error(
132 "A HybridGaussianConditional unexpectedly contained a non-conditional");
138 const This *
e =
dynamic_cast<const This *
>(&lf);
139 if (
e ==
nullptr)
return false;
150 return f1->equals(*(f2), tol);
157 std::cout << (
s.empty() ?
"" :
s +
"\n");
160 if (
isHybrid()) std::cout <<
"Hybrid ";
162 std::cout <<
" Discrete Keys = ";
164 std::cout <<
"(" <<
formatter(dk.first) <<
", " << dk.second <<
"), ";
166 std::cout << std::endl
167 <<
" logNormalizationConstant: " << -
negLogConstant() << std::endl
173 if (gf && !gf->empty()) {
189 const Key key = discreteKey.first;
191 continuousParentKeys.erase(std::remove(continuousParentKeys.begin(),
192 continuousParentKeys.end(),
key),
193 continuousParentKeys.end());
195 return continuousParentKeys;
201 for (
auto &&kv : given) {
202 if (given.find(kv.first) == given.end()) {
213 throw std::runtime_error(
214 "HybridGaussianConditional::likelihood: given values are missing some "
224 const auto likelihood_m = conditional->likelihood(given);
225 const double Cgm_Kgcm = conditional->negLogConstant() -
negLogConstant_;
226 if (Cgm_Kgcm == 0.0) {
227 return {likelihood_m, 0.0};
231 return {likelihood_m, Cgm_Kgcm};
234 return std::make_shared<HybridGaussianFactor>(discreteParentKeys,
240 std::set<DiscreteKey>
s(discreteKeys.begin(), discreteKeys.end());
260 auto pruner = [discreteProbs, discreteProbsKeySet, hybridGaussianCondKeySet](
269 if (hybridGaussianCondKeySet == discreteProbsKeySet) {
270 if (discreteProbs(
values) == 0.0) {
272 std::shared_ptr<GaussianConditional>
null;
278 std::vector<DiscreteKey> set_diff;
280 discreteProbsKeySet.begin(), discreteProbsKeySet.end(),
281 hybridGaussianCondKeySet.begin(), hybridGaussianCondKeySet.end(),
282 std::back_inserter(set_diff));
284 const std::vector<DiscreteValues> assignments =
288 augmented_values.
insert(assignment);
292 if (discreteProbs(augmented_values) > 0.0) {
322 return conditional->logProbability(continuousValues);
336 return conditional->logProbability(
values.continuous());
342 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_
a decision tree of Gaussian 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
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.
ConstructorHelper(const Conditionals &conditionals)
Compute all variables needed for the private constructor below.
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::optional< size_t > nrFrontals
std::shared_ptr< HybridGaussianFactor > likelihood(const VectorValues &given) const
bool equals(const HybridFactor &lf, double tol=1e-9) const override
equals
const gtsam::Symbol key('X', 0)
GaussianFactorGraphTree asGaussianFactorGraphTree() const
Convert to a DecisionTree of Gaussian factor graphs.
std::set< DiscreteKey > DiscreteKeysAsSet(const DiscreteKeys &discreteKeys)
Return the DiscreteKey vector as a set.
DecisionTree< Key, GaussianConditional::shared_ptr > Conditionals
typedef for Decision Tree of Gaussian Conditionals
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
std::function< GaussianConditional::shared_ptr(const Assignment< Key > &, const GaussianConditional::shared_ptr &)> prunerFunc(const DecisionTreeFactor &prunedProbabilities)
Helper function to get the pruner functional.
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.
HybridGaussianFactor::FactorValuePairs pairs
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 Sat Sep 28 2024 03:00:50