HybridGaussianFactorGraph.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
21 #include <gtsam/base/utilities.h>
37 #include <gtsam/inference/Key.h>
44 
45 #include <cstddef>
46 #include <iostream>
47 #include <memory>
48 #include <stdexcept>
49 #include <utility>
50 #include <vector>
51 
52 namespace gtsam {
53 
55 template class EliminateableFactorGraph<HybridGaussianFactorGraph>;
56 
57 using std::dynamic_pointer_cast;
59 
61 struct Result {
62  // Gaussian conditional resulting from elimination.
64  double negLogK; // Negative log of the normalization constant K.
65  GaussianFactor::shared_ptr factor; // Leftover factor 𝜏.
66  double scalar; // Scalar value associated with factor 𝜏.
67 
68  bool operator==(const Result &other) const {
69  return conditional == other.conditional && negLogK == other.negLogK &&
70  factor == other.factor && scalar == other.scalar;
71  }
72 };
74 
75 static const VectorValues kEmpty;
76 
77 /* ************************************************************************ */
78 // Throw a runtime exception for method specified in string s, and factor f:
79 static void throwRuntimeError(const std::string &s,
80  const std::shared_ptr<Factor> &f) {
81  auto &fr = *f;
82  throw std::runtime_error(s + " not implemented for factor type " +
83  demangle(typeid(fr).name()) + ".");
84 }
85 
86 /* ************************************************************************ */
88  KeySet discrete_keys = graph.discreteKeySet();
89  const VariableIndex index(graph);
91  index, KeyVector(discrete_keys.begin(), discrete_keys.end()), true);
92 }
93 
94 /* ************************************************************************ */
95 static void printFactor(const std::shared_ptr<Factor> &factor,
96  const DiscreteValues &assignment,
97  const KeyFormatter &keyFormatter) {
98  if (auto hgf = dynamic_pointer_cast<HybridGaussianFactor>(factor)) {
99  if (assignment.empty()) {
100  hgf->print("HybridGaussianFactor:", keyFormatter);
101  } else {
102  hgf->operator()(assignment)
103  .first->print("HybridGaussianFactor, component:", keyFormatter);
104  }
105  } else if (auto gf = dynamic_pointer_cast<GaussianFactor>(factor)) {
106  factor->print("GaussianFactor:\n", keyFormatter);
107 
108  } else if (auto df = dynamic_pointer_cast<DiscreteFactor>(factor)) {
109  factor->print("DiscreteFactor:\n", keyFormatter);
110  } else if (auto hc = dynamic_pointer_cast<HybridConditional>(factor)) {
111  if (hc->isContinuous()) {
112  factor->print("GaussianConditional:\n", keyFormatter);
113  } else if (hc->isDiscrete()) {
114  factor->print("DiscreteConditional:\n", keyFormatter);
115  } else {
116  if (assignment.empty()) {
117  hc->print("HybridConditional:", keyFormatter);
118  } else {
119  hc->asHybrid()
120  ->choose(assignment)
121  ->print("HybridConditional, component:\n", keyFormatter);
122  }
123  }
124  } else {
125  factor->print("Unknown factor type\n", keyFormatter);
126  }
127 }
128 
129 /* ************************************************************************ */
130 void HybridGaussianFactorGraph::print(const std::string &s,
131  const KeyFormatter &keyFormatter) const {
132  std::cout << (s.empty() ? "" : s + " ") << std::endl;
133  std::cout << "size: " << size() << std::endl;
134 
135  for (size_t i = 0; i < factors_.size(); i++) {
136  auto &&factor = factors_[i];
137  if (factor == nullptr) {
138  std::cout << "Factor " << i << ": nullptr\n";
139  continue;
140  }
141  // Print the factor
142  std::cout << "Factor " << i << "\n";
143  printFactor(factor, {}, keyFormatter);
144  std::cout << "\n";
145  }
146  std::cout.flush();
147 }
148 
149 /* ************************************************************************ */
151  const HybridValues &values, const std::string &str,
152  const KeyFormatter &keyFormatter,
153  const std::function<bool(const Factor * /*factor*/,
154  double /*whitenedError*/, size_t /*index*/)>
155  &printCondition) const {
156  std::cout << str << "size: " << size() << std::endl << std::endl;
157 
158  for (size_t i = 0; i < factors_.size(); i++) {
159  auto &&factor = factors_[i];
160  if (factor == nullptr) {
161  std::cout << "Factor " << i << ": nullptr\n";
162  continue;
163  }
164  const double errorValue = factor->error(values);
165  if (!printCondition(factor.get(), errorValue, i))
166  continue; // User-provided filter did not pass
167 
168  // Print the factor
169  std::cout << "Factor " << i << ", error = " << errorValue << "\n";
170  printFactor(factor, values.discrete(), keyFormatter);
171  std::cout << "\n";
172  }
173  std::cout.flush();
174 }
175 
176 /* ************************************************************************ */
178  const {
180 
181  for (auto &f : factors_) {
182  // TODO(dellaert): can we make this cleaner and less error-prone?
183  if (auto orphan = dynamic_pointer_cast<OrphanWrapper>(f)) {
184  continue; // Ignore OrphanWrapper
185  } else if (auto gf = dynamic_pointer_cast<GaussianFactor>(f)) {
186  result += gf;
187  } else if (auto gc = dynamic_pointer_cast<GaussianConditional>(f)) {
188  result += gc;
189  } else if (auto gmf = dynamic_pointer_cast<HybridGaussianFactor>(f)) {
190  result += *gmf;
191  } else if (auto gm = dynamic_pointer_cast<HybridGaussianConditional>(f)) {
192  result += *gm; // handled above already?
193  } else if (auto hc = dynamic_pointer_cast<HybridConditional>(f)) {
194  if (auto gm = hc->asHybrid()) {
195  result += *gm;
196  } else if (auto g = hc->asGaussian()) {
197  result += g;
198  } else {
199  // Has to be discrete.
200  // TODO(dellaert): in C++20, we can use std::visit.
201  continue;
202  }
203  } else if (dynamic_pointer_cast<DiscreteFactor>(f)) {
204  // Don't do anything for discrete-only factors
205  // since we want to eliminate continuous values only.
206  continue;
207  } else {
208  // TODO(dellaert): there was an unattributed comment here: We need to
209  // handle the case where the object is actually an BayesTreeOrphanWrapper!
210  throwRuntimeError("gtsam::collectProductFactor", f);
211  }
212  }
213 
214  return result;
215 }
216 
217 /* ************************************************************************ */
218 static std::pair<HybridConditional::shared_ptr, std::shared_ptr<Factor>>
220  const Ordering &frontalKeys) {
222  for (auto &f : factors) {
223  if (auto gf = dynamic_pointer_cast<GaussianFactor>(f)) {
224  gfg.push_back(gf);
225  } else if (auto orphan = dynamic_pointer_cast<OrphanWrapper>(f)) {
226  // Ignore orphaned clique.
227  // TODO(dellaert): is this correct? If so explain here.
228  } else if (auto hc = dynamic_pointer_cast<HybridConditional>(f)) {
229  auto gc = hc->asGaussian();
230  if (!gc) throwRuntimeError("continuousElimination", gc);
231  gfg.push_back(gc);
232  } else {
233  throwRuntimeError("continuousElimination", f);
234  }
235  }
236 
237  auto result = EliminatePreferCholesky(gfg, frontalKeys);
238  return {std::make_shared<HybridConditional>(result.first), result.second};
239 }
240 
241 /* ************************************************************************ */
250  const DiscreteKeys &discreteKeys,
251  const AlgebraicDecisionTree<Key> &errors) {
252  double min_log = errors.min();
253  AlgebraicDecisionTree<Key> potentials(
254  errors, [&min_log](const double x) { return exp(-(x - min_log)); });
255  return std::make_shared<DecisionTreeFactor>(discreteKeys, potentials);
256 }
257 
258 /* ************************************************************************ */
259 static std::pair<HybridConditional::shared_ptr, std::shared_ptr<Factor>>
261  const Ordering &frontalKeys) {
263 
264  for (auto &f : factors) {
265  if (auto df = dynamic_pointer_cast<DiscreteFactor>(f)) {
266  dfg.push_back(df);
267  } else if (auto gmf = dynamic_pointer_cast<HybridGaussianFactor>(f)) {
268  // Case where we have a HybridGaussianFactor with no continuous keys.
269  // In this case, compute a discrete factor from the remaining error.
270  auto calculateError = [&](const auto &pair) -> double {
271  auto [factor, scalar] = pair;
272  // If factor is null, it has been pruned, hence return infinite error
273  if (!factor) return std::numeric_limits<double>::infinity();
274  return scalar + factor->error(kEmpty);
275  };
276  AlgebraicDecisionTree<Key> errors(gmf->factors(), calculateError);
277  dfg.push_back(DiscreteFactorFromErrors(gmf->discreteKeys(), errors));
278 
279  } else if (auto orphan = dynamic_pointer_cast<OrphanWrapper>(f)) {
280  // Ignore orphaned clique.
281  // TODO(dellaert): is this correct? If so explain here.
282  } else if (auto hc = dynamic_pointer_cast<HybridConditional>(f)) {
283  auto dc = hc->asDiscrete();
284  if (!dc) throwRuntimeError("discreteElimination", dc);
285  dfg.push_back(hc->asDiscrete());
286  } else {
287  throwRuntimeError("discreteElimination", f);
288  }
289  }
290 
291  // NOTE: This does sum-product. For max-product, use EliminateForMPE.
292  auto result = EliminateDiscrete(dfg, frontalKeys);
293 
294  return {std::make_shared<HybridConditional>(result.first), result.second};
295 }
296 
297 /* ************************************************************************ */
304 static std::shared_ptr<Factor> createDiscreteFactor(
305  const ResultTree &eliminationResults,
306  const DiscreteKeys &discreteSeparator) {
307  auto calculateError = [&](const Result &result) -> double {
308  if (result.conditional && result.factor) {
309  // `error` has the following contributions:
310  // - the scalar is the sum of all mode-dependent constants
311  // - factor->error(kempty) is the error remaining after elimination
312  // - negLogK is what is given to the conditional to normalize
313  return result.scalar + result.factor->error(kEmpty) - result.negLogK;
314  } else if (!result.conditional && !result.factor) {
315  // If the factor has been pruned, return infinite error
316  return std::numeric_limits<double>::infinity();
317  } else {
318  throw std::runtime_error("createDiscreteFactor has mixed NULLs");
319  }
320  };
321 
322  AlgebraicDecisionTree<Key> errors(eliminationResults, calculateError);
323  return DiscreteFactorFromErrors(discreteSeparator, errors);
324 }
325 
326 /* *******************************************************************************/
327 // Create HybridGaussianFactor on the separator, taking care to correct
328 // for conditional constants.
329 static std::shared_ptr<Factor> createHybridGaussianFactor(
330  const ResultTree &eliminationResults,
331  const DiscreteKeys &discreteSeparator) {
332  // Correct for the normalization constant used up by the conditional
333  auto correct = [&](const Result &result) -> GaussianFactorValuePair {
334  if (result.conditional && result.factor) {
335  return {result.factor, result.scalar - result.negLogK};
336  } else if (!result.conditional && !result.factor) {
337  return {nullptr, std::numeric_limits<double>::infinity()};
338  } else {
339  throw std::runtime_error("createHybridGaussianFactors has mixed NULLs");
340  }
341  };
342  DecisionTree<Key, GaussianFactorValuePair> newFactors(eliminationResults,
343  correct);
344 
345  return std::make_shared<HybridGaussianFactor>(discreteSeparator, newFactors);
346 }
347 
348 /* *******************************************************************************/
350 static auto GetDiscreteKeys =
351  [](const HybridGaussianFactorGraph &hfg) -> DiscreteKeys {
352  const std::set<DiscreteKey> discreteKeySet = hfg.discreteKeys();
353  return {discreteKeySet.begin(), discreteKeySet.end()};
354 };
355 
356 /* *******************************************************************************/
357 std::pair<HybridConditional::shared_ptr, std::shared_ptr<Factor>>
359  // Since we eliminate all continuous variables first,
360  // the discrete separator will be *all* the discrete keys.
361  DiscreteKeys discreteSeparator = GetDiscreteKeys(*this);
362 
363  // Collect all the factors to create a set of Gaussian factor graphs in a
364  // decision tree indexed by all discrete keys involved. Just like any hybrid
365  // factor, every assignment also has a scalar error, in this case the sum of
366  // all errors in the graph. This error is assignment-specific and accounts for
367  // any difference in noise models used.
369 
370  // Check if a factor is null
371  auto isNull = [](const GaussianFactor::shared_ptr &ptr) { return !ptr; };
372 
373  // This is the elimination method on the leaf nodes
374  bool someContinuousLeft = false;
375  auto eliminate =
376  [&](const std::pair<GaussianFactorGraph, double> &pair) -> Result {
377  const auto &[graph, scalar] = pair;
378 
379  // If any product contains a pruned factor, prune it here. Done here as it's
380  // non non-trivial to do within collectProductFactor.
381  if (graph.empty() || std::any_of(graph.begin(), graph.end(), isNull)) {
382  return {nullptr, 0.0, nullptr, 0.0};
383  }
384 
385  // Expensive elimination of product factor.
386  auto [conditional, factor] =
388 
389  // Record whether there any continuous variables left
390  someContinuousLeft |= !factor->empty();
391 
392  // We pass on the scalar unmodified.
393  return {conditional, conditional->negLogConstant(), factor, scalar};
394  };
395 
396  // Perform elimination!
397  const ResultTree eliminationResults(productFactor, eliminate);
398 
399  // If there are no more continuous parents we create a DiscreteFactor with the
400  // error for each discrete choice. Otherwise, create a HybridGaussianFactor
401  // on the separator, taking care to correct for conditional constants.
402  auto newFactor =
403  someContinuousLeft
404  ? createHybridGaussianFactor(eliminationResults, discreteSeparator)
405  : createDiscreteFactor(eliminationResults, discreteSeparator);
406 
407  // Create the HybridGaussianConditional without re-calculating constants:
409  eliminationResults, [](const Result &result) -> GaussianFactorValuePair {
410  return {result.conditional, result.negLogK};
411  });
412  auto hybridGaussian =
413  std::make_shared<HybridGaussianConditional>(discreteSeparator, pairs);
414 
415  return {std::make_shared<HybridConditional>(hybridGaussian), newFactor};
416 }
417 
418 /* ************************************************************************
419  * Function to eliminate variables **under the following assumptions**:
420  * 1. When the ordering is fully continuous, and the graph only contains
421  * continuous and hybrid factors
422  * 2. When the ordering is fully discrete, and the graph only contains discrete
423  * factors
424  *
425  * Any usage outside of this is considered incorrect.
426  *
427  * \warning This function is not meant to be used with arbitrary hybrid factor
428  * graphs. For example, if there exists continuous parents, and one tries to
429  * eliminate a discrete variable (as specified in the ordering), the result will
430  * be INCORRECT and there will be NO error raised.
431  */
432 std::pair<HybridConditional::shared_ptr, std::shared_ptr<Factor>> //
434  const Ordering &keys) {
435  // NOTE: Because we are in the Conditional Gaussian regime there are only
436  // a few cases:
437  // 1. continuous variable, make a hybrid Gaussian conditional if there are
438  // hybrid factors;
439  // 2. continuous variable, we make a Gaussian Factor if there are no hybrid
440  // factors;
441  // 3. discrete variable, no continuous factor is allowed
442  // (escapes Conditional Gaussian regime), if discrete only we do the discrete
443  // elimination.
444 
445  // However it is not that simple. During elimination it is possible that the
446  // multifrontal needs to eliminate an ordering that contains both Gaussian and
447  // hybrid variables, for example x1, c1.
448  // In this scenario, we will have a density P(x1, c1) that is a Conditional
449  // Linear Gaussian P(x1|c1)P(c1) (see Murphy02).
450 
451  // The issue here is that, how can we know which variable is discrete if we
452  // unify Values? Obviously we can tell using the factors, but is that fast?
453 
454  // In the case of multifrontal, we will need to use a constrained ordering
455  // so that the discrete parts will be guaranteed to be eliminated last!
456  // Because of all these reasons, we carefully consider how to
457  // implement the hybrid factors so that we do not get poor performance.
458 
459  // The first thing is how to represent the HybridGaussianConditional.
460  // A very possible scenario is that the incoming factors will have different
461  // levels of discrete keys. For example, imagine we are going to eliminate the
462  // fragment: $\phi(x1,c1,c2)$, $\phi(x1,c2,c3)$, which is perfectly valid.
463  // Now we will need to know how to retrieve the corresponding continuous
464  // densities for the assignment (c1,c2,c3) (OR (c2,c3,c1), note there is NO
465  // defined order!). We also need to consider when there is pruning. Two
466  // hybrid factors could have different pruning patterns - one could have
467  // (c1=0,c2=1) pruned, and another could have (c2=0,c3=1) pruned, and this
468  // creates a big problem in how to identify the intersection of non-pruned
469  // branches.
470 
471  // Our approach is first building the collection of all discrete keys. After
472  // that we enumerate the space of all key combinations *lazily* so that the
473  // exploration branch terminates whenever an assignment yields NULL in any of
474  // the hybrid factors.
475 
476  // When the number of assignments is large we may encounter stack overflows.
477  // However this is also the case with iSAM2, so no pressure :)
478 
479  // Check the factors:
480  // 1. if all factors are discrete, then we can do discrete elimination:
481  // 2. if all factors are continuous, then we can do continuous elimination:
482  // 3. if not, we do hybrid elimination:
483 
484  bool only_discrete = true, only_continuous = true;
485  for (auto &&factor : factors) {
486  if (auto hybrid_factor = std::dynamic_pointer_cast<HybridFactor>(factor)) {
487  if (hybrid_factor->isDiscrete()) {
488  only_continuous = false;
489  } else if (hybrid_factor->isContinuous()) {
490  only_discrete = false;
491  } else if (hybrid_factor->isHybrid()) {
492  only_continuous = false;
493  only_discrete = false;
494  break;
495  }
496  } else if (auto cont_factor =
497  std::dynamic_pointer_cast<GaussianFactor>(factor)) {
498  only_discrete = false;
499  } else if (auto discrete_factor =
500  std::dynamic_pointer_cast<DiscreteFactor>(factor)) {
501  only_continuous = false;
502  }
503  }
504 
505  // NOTE: We should really defer the product here because of pruning
506 
507  if (only_discrete) {
508  // Case 1: we are only dealing with discrete
510  } else if (only_continuous) {
511  // Case 2: we are only dealing with continuous
513  } else {
514  // Case 3: We are now in the hybrid land!
515  return factors.eliminate(keys);
516  }
517 }
518 
519 /* ************************************************************************ */
521  const VectorValues &continuousValues) const {
523  // Iterate over each factor.
524  for (auto &factor : factors_) {
525  if (auto hf = std::dynamic_pointer_cast<HybridFactor>(factor)) {
526  // Add errorTree for hybrid factors, includes HybridGaussianConditionals!
527  result = result + hf->errorTree(continuousValues);
528  } else if (auto df = std::dynamic_pointer_cast<DiscreteFactor>(factor)) {
529  // If discrete, just add its errorTree as well
530  result = result + df->errorTree();
531  } else if (auto gf = dynamic_pointer_cast<GaussianFactor>(factor)) {
532  // For a continuous only factor, just add its error
533  result = result + gf->error(continuousValues);
534  } else {
535  throwRuntimeError("HybridGaussianFactorGraph::errorTree", factor);
536  }
537  }
538  return result;
539 }
540 
541 /* ************************************************************************ */
543  double error = this->error(values);
544  // NOTE: The 0.5 term is handled by each factor
545  return std::exp(-error);
546 }
547 
548 /* ************************************************************************ */
550  const VectorValues &continuousValues) const {
551  AlgebraicDecisionTree<Key> errors = this->errorTree(continuousValues);
552  AlgebraicDecisionTree<Key> p = errors.apply([](double error) {
553  // NOTE: The 0.5 term is handled by each factor
554  return exp(-error);
555  });
556  return p / p.sum();
557 }
558 
559 /* ************************************************************************ */
561  const DiscreteValues &assignment) const {
563  for (auto &&f : *this) {
564  if (auto gf = std::dynamic_pointer_cast<GaussianFactor>(f)) {
565  gfg.push_back(gf);
566  } else if (auto gc = std::dynamic_pointer_cast<GaussianConditional>(f)) {
567  gfg.push_back(gf);
568  } else if (auto hgf = std::dynamic_pointer_cast<HybridGaussianFactor>(f)) {
569  gfg.push_back((*hgf)(assignment).first);
570  } else if (auto hgc = dynamic_pointer_cast<HybridGaussianConditional>(f)) {
571  gfg.push_back((*hgc)(assignment));
572  } else if (auto hc = dynamic_pointer_cast<HybridConditional>(f)) {
573  if (auto gc = hc->asGaussian())
574  gfg.push_back(gc);
575  else if (auto hgc = hc->asHybrid())
576  gfg.push_back((*hgc)(assignment));
577  } else {
578  continue;
579  }
580  }
581  return gfg;
582 }
583 
584 /* ************************************************************************ */
587  for (auto &&f : factors_) {
588  if (auto discreteFactor = std::dynamic_pointer_cast<DiscreteFactor>(f)) {
589  dfg.push_back(discreteFactor);
590  }
591  }
592  return dfg;
593 }
594 
595 } // namespace gtsam
gtsam::BayesTreeOrphanWrapper
Definition: BayesTree.h:281
gtsam::EliminateDiscrete
std::pair< DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr > EliminateDiscrete(const DiscreteFactorGraph &factors, const Ordering &frontalKeys)
Main elimination function for DiscreteFactorGraph.
Definition: DiscreteFactorGraph.cpp:219
HybridJunctionTree.h
gtsam::Ordering::ColamdConstrainedLast
static Ordering ColamdConstrainedLast(const FACTOR_GRAPH &graph, const KeyVector &constrainLast, bool forceOrder=false)
Definition: inference/Ordering.h:112
gtsam::Result
Result from elimination.
Definition: HybridGaussianFactorGraph.cpp:61
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
gtsam::HybridValues
Definition: HybridValues.h:37
GaussianConditional.h
Conditional Gaussian Base class.
gtsam::FactorGraph< Factor >::factors_
FastVector< sharedFactor > factors_
Definition: FactorGraph.h:92
gtsam::DiscreteFactorGraph
Definition: DiscreteFactorGraph.h:99
gtsam::Result::conditional
GaussianConditional::shared_ptr conditional
Definition: HybridGaussianFactorGraph.cpp:63
HybridGaussianConditional.h
A hybrid conditional in the Conditional Linear Gaussian scheme.
s
RealScalar s
Definition: level1_cplx_impl.h:126
gtsam::FactorGraph< Factor >::error
double error(const HybridValues &values) const
Definition: FactorGraph-inst.h:66
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
DiscreteFactorGraph.h
simple_graph::factors
const GaussianFactorGraph factors
Definition: testJacobianFactor.cpp:213
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
EliminateableFactorGraph-inst.h
gtsam::GetDiscreteKeys
static auto GetDiscreteKeys
Get the discrete keys from the HybridGaussianFactorGraph as DiscreteKeys.
Definition: HybridGaussianFactorGraph.cpp:350
gtsam::DecisionTreeFactor::error
double error(const DiscreteValues &values) const override
Calculate error for DiscreteValues x, is -log(probability).
Definition: DecisionTreeFactor.cpp:56
gtsam::HybridGaussianProductFactor
Alias for DecisionTree of GaussianFactorGraphs and their scalar sums.
Definition: HybridGaussianProductFactor.h:34
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::Factor
Definition: Factor.h:70
gtsam::DiscreteKeys
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition: DiscreteKey.h:41
gtsam::FastSet
Definition: FastSet.h:51
any_of
negation< all_of< negation< Ts >... > > any_of
Definition: wrap/pybind11/include/pybind11/detail/common.h:742
GaussianJunctionTree.h
exp
const EIGEN_DEVICE_FUNC ExpReturnType exp() const
Definition: ArrayCwiseUnaryOps.h:97
gtsam::HybridGaussianFactorGraph::discreteFactors
DiscreteFactorGraph discreteFactors() const
Helper method to get all the discrete factors as a DiscreteFactorGraph.
Definition: HybridGaussianFactorGraph.cpp:585
gtsam::DecisionTreeFactor::shared_ptr
std::shared_ptr< DecisionTreeFactor > shared_ptr
Definition: DecisionTreeFactor.h:51
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
result
Values result
Definition: OdometryOptimize.cpp:8
HybridGaussianFactor.h
A set of GaussianFactors, indexed by a set of discrete keys.
gtsam::HybridGaussianFactorGraph::collectProductFactor
HybridGaussianProductFactor collectProductFactor() const
Create a decision tree of factor graphs out of this hybrid factor graph.
Definition: HybridGaussianFactorGraph.cpp:177
utilities.h
hc
Vector3d hc
Definition: Tridiagonalization_householderCoefficients.cpp:5
name
static char name[]
Definition: rgamma.c:72
gtsam::AlgebraicDecisionTree< Key >
pruning_fixture::factor
DecisionTreeFactor factor(D &C &B &A, "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 " "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0")
gtsam::EliminatePreferCholesky
std::pair< std::shared_ptr< GaussianConditional >, std::shared_ptr< GaussianFactor > > EliminatePreferCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
Definition: HessianFactor.cpp:541
gtsam::Result::scalar
double scalar
Definition: HybridGaussianFactorGraph.cpp:66
Key.h
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
GaussianEliminationTree.h
HybridFactor.h
gtsam::GaussianFactorValuePair
std::pair< GaussianFactor::shared_ptr, double > GaussianFactorValuePair
Alias for pair of GaussianFactor::shared_pointer and a double value.
Definition: HybridGaussianFactor.h:38
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::throwRuntimeError
static void throwRuntimeError(const std::string &s, const std::shared_ptr< Factor > &f)
Definition: HybridGaussianFactorGraph.cpp:79
gtsam::createDiscreteFactor
static std::shared_ptr< Factor > createDiscreteFactor(const ResultTree &eliminationResults, const DiscreteKeys &discreteSeparator)
Definition: HybridGaussianFactorGraph.cpp:304
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
DiscreteJunctionTree.h
scalar
mxArray * scalar(mxClassID classid)
Definition: matlab.h:82
gtsam::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::continuousElimination
static std::pair< HybridConditional::shared_ptr, std::shared_ptr< Factor > > continuousElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys)
Definition: HybridGaussianFactorGraph.cpp:219
gtsam::FactorGraph< Factor >::keys
KeySet keys() const
Definition: FactorGraph-inst.h:85
Assignment.h
An assignment from labels to a discrete value index (size_t)
gtsam::discreteElimination
static std::pair< HybridConditional::shared_ptr, std::shared_ptr< Factor > > discreteElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys)
Definition: HybridGaussianFactorGraph.cpp:260
gtsam::HybridGaussianFactorGraph::eliminate
std::pair< std::shared_ptr< HybridConditional >, std::shared_ptr< Factor > > eliminate(const Ordering &keys) const
Eliminate the given continuous keys.
Definition: HybridGaussianFactorGraph.cpp:358
gtsam::HybridGaussianFactorGraph
Definition: HybridGaussianFactorGraph.h:106
gtsam::kEmpty
static const VectorValues kEmpty
Definition: HybridGaussianFactorGraph.cpp:75
gtsam::DecisionTreeFactor::print
void print(const std::string &s="DecisionTreeFactor:\n", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print
Definition: DecisionTreeFactor.cpp:74
gtsam::EliminateHybrid
std::pair< HybridConditional::shared_ptr, std::shared_ptr< Factor > > EliminateHybrid(const HybridGaussianFactorGraph &factors, const Ordering &keys)
Main elimination function for HybridGaussianFactorGraph.
Definition: HybridGaussianFactorGraph.cpp:433
gtsam::printFactor
static void printFactor(const std::shared_ptr< Factor > &factor, const DiscreteValues &assignment, const KeyFormatter &keyFormatter)
Definition: HybridGaussianFactorGraph.cpp:95
gtsam::DecisionTree::apply
DecisionTree apply(const Unary &op) const
Definition: DecisionTree-inl.h:1000
DiscreteEliminationTree.h
gtsam::VariableIndex
Definition: VariableIndex.h:41
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
DiscreteKey.h
specialized key for discrete variables
str
Definition: pytypes.h:1558
gtsam::Result::operator==
bool operator==(const Result &other) const
Definition: HybridGaussianFactorGraph.cpp:68
JacobianFactor.h
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam::FactorGraph< Factor >::size
size_t size() const
Definition: FactorGraph.h:297
gtsam::DecisionTree
a decision tree is a function from assignments to values.
Definition: DecisionTree.h:62
HybridEliminationTree.h
different_sigmas::gc
const auto gc
Definition: testHybridBayesNet.cpp:231
HybridGaussianFactorGraph.h
Linearized Hybrid factor graph that uses type erasure.
gtsam
traits
Definition: SFMdata.h:40
gtsam::HybridGaussianFactorGraph::choose
GaussianFactorGraph choose(const DiscreteValues &assignment) const
Get the GaussianFactorGraph at a given discrete assignment. Note this corresponds to the Gaussian pos...
Definition: HybridGaussianFactorGraph.cpp:560
DiscreteValues.h
gtsam::HybridGaussianFactorGraph::printErrors
void printErrors(const HybridValues &values, const std::string &str="HybridGaussianFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter, const std::function< bool(const Factor *, double, size_t)> &printCondition=[](const Factor *, double, size_t) { return true;}) const
Print the errors of each factor in the hybrid factor graph.
Definition: HybridGaussianFactorGraph.cpp:150
gtsam::HybridGaussianFactorGraph::probPrime
double probPrime(const HybridValues &values) const
Compute the unnormalized posterior probability for a continuous vector values given a specific assign...
Definition: HybridGaussianFactorGraph.cpp:542
gtsam::DiscreteValues
Definition: DiscreteValues.h:34
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:147
gtsam::createHybridGaussianFactor
static std::shared_ptr< Factor > createHybridGaussianFactor(const ResultTree &eliminationResults, const DiscreteKeys &discreteSeparator)
Definition: HybridGaussianFactorGraph.cpp:329
gtsam::demangle
std::string demangle(const char *name)
Pretty print Value type name.
Definition: types.cpp:37
gtsam::Result::factor
GaussianFactor::shared_ptr factor
Definition: HybridGaussianFactorGraph.cpp:65
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::GaussianConditional::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianConditional.h:46
gtsam::HybridGaussianFactorGraph::errorTree
AlgebraicDecisionTree< Key > errorTree(const VectorValues &continuousValues) const
Compute error for each discrete assignment, and return as a tree.
Definition: HybridGaussianFactorGraph.cpp:520
gtsam::HybridGaussianFactorGraph::print
void print(const std::string &s="HybridGaussianFactorGraph", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print out graph to std::cout, with optional key formatter.
Definition: HybridGaussianFactorGraph.cpp:130
gtsam::HybridOrdering
const Ordering HybridOrdering(const HybridGaussianFactorGraph &graph)
Return a Colamd constrained ordering where the discrete keys are eliminated after the continuous keys...
Definition: HybridGaussianFactorGraph.cpp:87
gtsam::AlgebraicDecisionTree::min
double min() const
Find the minimum values amongst all leaves.
Definition: AlgebraicDecisionTree.h:224
gtsam::DiscreteFactorFromErrors
static DecisionTreeFactor::shared_ptr DiscreteFactorFromErrors(const DiscreteKeys &discreteKeys, const AlgebraicDecisionTree< Key > &errors)
Take negative log-values, shift them so that the minimum value is 0, and then exponentiate to create ...
Definition: HybridGaussianFactorGraph.cpp:249
different_sigmas::hgc
const auto hgc
Definition: testHybridBayesNet.cpp:236
gtsam::HybridGaussianFactorGraph::discretePosterior
AlgebraicDecisionTree< Key > discretePosterior(const VectorValues &continuousValues) const
Computer posterior P(M|X=x) when all continuous values X are given. This is efficient as this simply ...
Definition: HybridGaussianFactorGraph.cpp:549
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::Ordering
Definition: inference/Ordering.h:33
DecisionTreeFactor.h
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
HybridConditional.h
gtsam::Result::negLogK
double negLogK
Definition: HybridGaussianFactorGraph.cpp:64


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:11:40