Go to the documentation of this file.
40 if (
auto conditional =
41 std::dynamic_pointer_cast<GaussianConditional>(factor)) {
44 throw std::logic_error(
45 "A HybridGaussianConditional unexpectedly contained a non-conditional");
65 using P = std::vector<std::pair<Vector, double>>;
68 template <
typename... Args>
71 std::vector<GaussianFactorValuePair> fvs;
72 std::vector<GC::shared_ptr> gcs;
73 fvs.reserve(
p.size());
74 gcs.reserve(
p.size());
76 auto gaussianConditional =
78 double value = gaussianConditional->negLogConstant();
80 fvs.emplace_back(gaussianConditional,
value);
81 gcs.push_back(gaussianConditional);
90 if (!
gc)
return {
nullptr, std::numeric_limits<double>::infinity()};
92 double value =
gc->negLogConstant();
98 throw std::runtime_error(
99 "HybridGaussianConditional: need at least one frontal variable. "
100 "Provided conditionals do not contain any frontal variables.");
107 if (!pair.first)
return;
114 throw std::runtime_error(
115 "HybridGaussianConditional: need at least one frontal variable. "
116 "Provided conditionals do not contain any frontal variables.");
129 pair.first, pair.second - helper.minNegLogConstant};
131 std::move(helper.pairs))),
132 BaseConditional(*helper.nrFrontals),
133 negLogConstant_(helper.minNegLogConstant) {}
137 const std::vector<GaussianConditional::shared_ptr> &
conditionals)
143 const std::vector<std::pair<Vector, double>> &
parameters)
150 const std::vector<std::pair<Vector, double>> &
parameters)
158 const std::vector<std::pair<Vector, double>> &
parameters)
176 return std::dynamic_pointer_cast<GaussianConditional>(pair.first);
184 if (node.first) total += 1;
192 auto &[factor,
_] =
factors()(discreteValues);
193 if (!factor)
return nullptr;
202 const This *
e =
dynamic_cast<const This *
>(&lf);
203 if (
e ==
nullptr)
return false;
210 auto c1 = std::dynamic_pointer_cast<GaussianConditional>(pair1.first),
211 c2 = std::dynamic_pointer_cast<GaussianConditional>(pair2.first);
220 std::cout << (
s.empty() ?
"" :
s +
"\n");
222 std::cout <<
" Discrete Keys = ";
224 std::cout <<
"(" <<
formatter(dk.first) <<
", " << dk.second <<
"), ";
226 std::cout << std::endl
227 <<
" logNormalizationConstant: " << -
negLogConstant() << std::endl
234 std::dynamic_pointer_cast<GaussianConditional>(pair.first)) {
250 const Key key = discreteKey.first;
252 continuousParentKeys.erase(std::remove(continuousParentKeys.begin(),
253 continuousParentKeys.end(),
key),
254 continuousParentKeys.end());
256 return continuousParentKeys;
262 for (
auto &&kv : given) {
263 if (given.find(kv.first) == given.end()) {
274 throw std::runtime_error(
275 "HybridGaussianConditional::likelihood: given values are missing some "
284 if (
auto conditional =
285 std::dynamic_pointer_cast<GaussianConditional>(pair.first)) {
286 const auto likelihood_m = conditional->likelihood(given);
288 return {likelihood_m, pair.second};
290 return {
nullptr, std::numeric_limits<double>::infinity()};
293 return std::make_shared<HybridGaussianFactor>(discreteParentKeys,
299 std::set<DiscreteKey>
s(discreteKeys.begin(), discreteKeys.end());
307 std::set<Key> mine(this->
keys().begin(), this->
keys().
end());
308 std::set<Key> theirs(discreteProbs.
keys().begin(),
309 discreteProbs.
keys().end());
310 std::vector<Key> diff;
311 std::set_difference(theirs.begin(), theirs.end(), mine.begin(), mine.end(),
312 std::back_inserter(diff));
323 if (
max->evaluate(choices) == 0.0)
324 return {
nullptr, std::numeric_limits<double>::infinity()};
328 return {pair.first, pair.second + negLogConstant_};
333 return std::make_shared<HybridGaussianConditional>(discreteKeys(),
338 double HybridGaussianConditional::logProbability(
342 return conditional->logProbability(
values.continuous());
349 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.
Conditional Gaussian Base class.
Helper(const Conditionals &conditionals)
Construct from tree of GaussianConditionals.
std::shared_ptr< This > shared_ptr
A hybrid conditional in the Conditional Linear Gaussian scheme.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
GaussianConditional::shared_ptr choose(const DiscreteValues &discreteValues) const
Return the conditional Gaussian for the given discrete assignment.
Helper struct for constructing HybridGaussianConditional objects.
Helper(const DiscreteKey &mode, const P &p, Args &&...args)
Construct from a vector of mean and sigma pairs, plus extra args.
bool equals(const DecisionTree &other, const CompareFunc &compare=&DefaultCompare) const
const GaussianFactorGraph factors
const KeyFormatter & formatter
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.
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
static const double sigma
Double_ range(const Point2_ &p, const Point2_ &q)
GaussianConditional::shared_ptr checkConditional(const GaussianFactor::shared_ptr &factor)
std::pair< GaussianFactor::shared_ptr, double > GaussianFactorValuePair
Alias for pair of GaussianFactor::shared_pointer and a double value.
static const DiscreteKey mode(modeKey, 2)
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
static ConjugateGradientParameters parameters
size_t nrComponents() const
Returns the total number of continuous components.
std::shared_ptr< This > shared_ptr
shared_ptr to this class
std::vector< std::pair< Vector, double > > P
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 continuousParents() const
Returns the continuous keys among the parents.
const FactorValuePairs & factors() const
Getter for GaussianFactor decision tree.
std::shared_ptr< HybridGaussianFactor > likelihood(const VectorValues &given) const
std::optional< size_t > nrFrontals
const gtsam::Symbol key('X', 0)
std::set< DiscreteKey > DiscreteKeysAsSet(const DiscreteKeys &discreteKeys)
Return the DiscreteKey vector as a set.
const Conditionals conditionals() const
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.
Point3 mean(const CONTAINER &points)
mean
const KeyVector & keys() const
Access the factor's involved variable keys.
bool equals(const This &other, double tol=1e-9) const
check equality
std::pair< Key, size_t > DiscreteKey
shared_ptr max(size_t nrFrontals) const
Create new factor by maximizing over all values with the same separator.
bool allFrontalsGiven(const VectorValues &given) const
Check whether given has values for all frontal keys.
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...
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.
static const EIGEN_DEPRECATED end_t end
std::uint64_t Key
Integer nonlinear key type.
constexpr descr< N - 1 > _(char const (&text)[N])
Helper(const FactorValuePairs &pairs)
Construct from tree of factor/scalar pairs.
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.
std::string str() const
return the string
static shared_ptr sharedMeanAndStddev(Args &&... args)
Create shared pointer by forwarding arguments to fromMeanAndStddev.
gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:02:26