From aef273bce8299a561b5b9829125688116076bfc4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 13 Sep 2024 05:41:24 -0400 Subject: [PATCH] rename GaussianMixture to HybridGaussianConditional --- doc/Hybrid.lyx | 12 ++-- gtsam/hybrid/GaussianMixture.cpp | 58 +++++++++---------- gtsam/hybrid/GaussianMixture.h | 32 +++++----- gtsam/hybrid/HybridBayesNet.cpp | 8 +-- gtsam/hybrid/HybridBayesNet.h | 4 +- gtsam/hybrid/HybridConditional.cpp | 2 +- gtsam/hybrid/HybridConditional.h | 18 +++--- gtsam/hybrid/HybridFactor.h | 2 +- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 14 ++--- gtsam/hybrid/HybridNonlinearFactorGraph.cpp | 6 +- gtsam/hybrid/HybridSmoother.cpp | 2 +- gtsam/hybrid/HybridSmoother.h | 2 +- gtsam/hybrid/hybrid.i | 12 ++-- gtsam/hybrid/tests/TinyHybridExample.h | 2 +- gtsam/hybrid/tests/testGaussianMixture.cpp | 28 ++++----- .../tests/testGaussianMixtureFactor.cpp | 12 ++-- gtsam/hybrid/tests/testHybridBayesNet.cpp | 4 +- gtsam/hybrid/tests/testHybridConditional.cpp | 4 +- gtsam/hybrid/tests/testHybridEstimation.cpp | 4 +- .../tests/testHybridGaussianFactorGraph.cpp | 20 +++---- gtsam/hybrid/tests/testHybridGaussianISAM.cpp | 16 ++--- .../tests/testHybridNonlinearFactorGraph.cpp | 2 +- .../hybrid/tests/testHybridNonlinearISAM.cpp | 16 ++--- .../hybrid/tests/testSerializationHybrid.cpp | 22 +++---- gtsam/inference/Conditional.h | 2 +- python/gtsam/tests/test_HybridBayesNet.py | 7 ++- python/gtsam/tests/test_HybridFactorGraph.py | 10 ++-- 27 files changed, 161 insertions(+), 160 deletions(-) diff --git a/doc/Hybrid.lyx b/doc/Hybrid.lyx index b5901d495..17d7094e1 100644 --- a/doc/Hybrid.lyx +++ b/doc/Hybrid.lyx @@ -191,13 +191,13 @@ E_{gc}(x,y)=\frac{1}{2}\|Rx+Sy-d\|_{\Sigma}^{2}.\label{eq:gc_error} \end_layout \begin_layout Subsubsection* -GaussianMixture +HybridGaussianConditional \end_layout \begin_layout Standard A \emph on -GaussianMixture +HybridGaussianConditional \emph default (maybe to be renamed to \emph on @@ -233,7 +233,7 @@ GaussianConditional to a set of discrete variables. As \emph on -GaussianMixture +HybridGaussianConditional \emph default is a \emph on @@ -324,7 +324,7 @@ The key point here is that \color inherit is the log-normalization constant for the complete \emph on -GaussianMixture +HybridGaussianConditional \emph default across all values of \begin_inset Formula $m$ @@ -556,7 +556,7 @@ Analogously, a \emph on HybridGaussianFactor \emph default - typically results from a GaussianMixture by having known values + typically results from a HybridGaussianConditional by having known values \begin_inset Formula $\bar{x}$ \end_inset @@ -817,7 +817,7 @@ E_{mf}(y,m)=\frac{1}{2}\|A_{m}y-b_{m}\|_{\Sigma_{mfm}}^{2}=E_{gcm}(\bar{x},y)+K_ \end_inset -which is identical to the GaussianMixture error +which is identical to the HybridGaussianConditional error \begin_inset CommandInset ref LatexCommand eqref reference "eq:gm_error" diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index f1f1ce429..451493ae0 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file GaussianMixture.cpp + * @file HybridGaussianConditional.cpp * @brief A hybrid conditional in the Conditional Linear Gaussian scheme * @author Fan Jiang * @author Varun Agrawal @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include #include @@ -29,10 +29,10 @@ namespace gtsam { -GaussianMixture::GaussianMixture( +HybridGaussianConditional::HybridGaussianConditional( const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, - const GaussianMixture::Conditionals &conditionals) + const HybridGaussianConditional::Conditionals &conditionals) : BaseFactor(CollectKeys(continuousFrontals, continuousParents), discreteParents), BaseConditional(continuousFrontals.size()), @@ -50,30 +50,30 @@ GaussianMixture::GaussianMixture( } /* *******************************************************************************/ -const GaussianMixture::Conditionals &GaussianMixture::conditionals() const { +const HybridGaussianConditional::Conditionals &HybridGaussianConditional::conditionals() const { return conditionals_; } /* *******************************************************************************/ -GaussianMixture::GaussianMixture( +HybridGaussianConditional::HybridGaussianConditional( KeyVector &&continuousFrontals, KeyVector &&continuousParents, DiscreteKeys &&discreteParents, std::vector &&conditionals) - : GaussianMixture(continuousFrontals, continuousParents, discreteParents, + : HybridGaussianConditional(continuousFrontals, continuousParents, discreteParents, Conditionals(discreteParents, conditionals)) {} /* *******************************************************************************/ -GaussianMixture::GaussianMixture( +HybridGaussianConditional::HybridGaussianConditional( const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, const std::vector &conditionals) - : GaussianMixture(continuousFrontals, continuousParents, discreteParents, + : HybridGaussianConditional(continuousFrontals, continuousParents, discreteParents, Conditionals(discreteParents, conditionals)) {} /* *******************************************************************************/ -// TODO(dellaert): This is copy/paste: GaussianMixture should be derived from +// TODO(dellaert): This is copy/paste: HybridGaussianConditional should be derived from // GaussianMixtureFactor, no? -GaussianFactorGraphTree GaussianMixture::add( +GaussianFactorGraphTree HybridGaussianConditional::add( const GaussianFactorGraphTree &sum) const { using Y = GaussianFactorGraph; auto add = [](const Y &graph1, const Y &graph2) { @@ -86,7 +86,7 @@ GaussianFactorGraphTree GaussianMixture::add( } /* *******************************************************************************/ -GaussianFactorGraphTree GaussianMixture::asGaussianFactorGraphTree() const { +GaussianFactorGraphTree HybridGaussianConditional::asGaussianFactorGraphTree() const { auto wrap = [this](const GaussianConditional::shared_ptr &gc) { // First check if conditional has not been pruned if (gc) { @@ -109,7 +109,7 @@ GaussianFactorGraphTree GaussianMixture::asGaussianFactorGraphTree() const { } /* *******************************************************************************/ -size_t GaussianMixture::nrComponents() const { +size_t HybridGaussianConditional::nrComponents() const { size_t total = 0; conditionals_.visit([&total](const GaussianFactor::shared_ptr &node) { if (node) total += 1; @@ -118,7 +118,7 @@ size_t GaussianMixture::nrComponents() const { } /* *******************************************************************************/ -GaussianConditional::shared_ptr GaussianMixture::operator()( +GaussianConditional::shared_ptr HybridGaussianConditional::operator()( const DiscreteValues &discreteValues) const { auto &ptr = conditionals_(discreteValues); if (!ptr) return nullptr; @@ -127,11 +127,11 @@ GaussianConditional::shared_ptr GaussianMixture::operator()( return conditional; else throw std::logic_error( - "A GaussianMixture unexpectedly contained a non-conditional"); + "A HybridGaussianConditional unexpectedly contained a non-conditional"); } /* *******************************************************************************/ -bool GaussianMixture::equals(const HybridFactor &lf, double tol) const { +bool HybridGaussianConditional::equals(const HybridFactor &lf, double tol) const { const This *e = dynamic_cast(&lf); if (e == nullptr) return false; @@ -149,7 +149,7 @@ bool GaussianMixture::equals(const HybridFactor &lf, double tol) const { } /* *******************************************************************************/ -void GaussianMixture::print(const std::string &s, +void HybridGaussianConditional::print(const std::string &s, const KeyFormatter &formatter) const { std::cout << (s.empty() ? "" : s + "\n"); if (isContinuous()) std::cout << "Continuous "; @@ -177,7 +177,7 @@ void GaussianMixture::print(const std::string &s, } /* ************************************************************************* */ -KeyVector GaussianMixture::continuousParents() const { +KeyVector HybridGaussianConditional::continuousParents() const { // Get all parent keys: const auto range = parents(); KeyVector continuousParentKeys(range.begin(), range.end()); @@ -193,7 +193,7 @@ KeyVector GaussianMixture::continuousParents() const { } /* ************************************************************************* */ -bool GaussianMixture::allFrontalsGiven(const VectorValues &given) const { +bool HybridGaussianConditional::allFrontalsGiven(const VectorValues &given) const { for (auto &&kv : given) { if (given.find(kv.first) == given.end()) { return false; @@ -203,11 +203,11 @@ bool GaussianMixture::allFrontalsGiven(const VectorValues &given) const { } /* ************************************************************************* */ -std::shared_ptr GaussianMixture::likelihood( +std::shared_ptr HybridGaussianConditional::likelihood( const VectorValues &given) const { if (!allFrontalsGiven(given)) { throw std::runtime_error( - "GaussianMixture::likelihood: given values are missing some frontals."); + "HybridGaussianConditional::likelihood: given values are missing some frontals."); } const DiscreteKeys discreteParentKeys = discreteKeys(); @@ -252,7 +252,7 @@ std::set DiscreteKeysAsSet(const DiscreteKeys &discreteKeys) { */ std::function &, const GaussianConditional::shared_ptr &)> -GaussianMixture::prunerFunc(const DecisionTreeFactor &discreteProbs) { +HybridGaussianConditional::prunerFunc(const DecisionTreeFactor &discreteProbs) { // Get the discrete keys as sets for the decision tree // and the gaussian mixture. auto discreteProbsKeySet = DiscreteKeysAsSet(discreteProbs.discreteKeys()); @@ -303,7 +303,7 @@ GaussianMixture::prunerFunc(const DecisionTreeFactor &discreteProbs) { } /* *******************************************************************************/ -void GaussianMixture::prune(const DecisionTreeFactor &discreteProbs) { +void HybridGaussianConditional::prune(const DecisionTreeFactor &discreteProbs) { // Functional which loops over all assignments and create a set of // GaussianConditionals auto pruner = prunerFunc(discreteProbs); @@ -313,7 +313,7 @@ void GaussianMixture::prune(const DecisionTreeFactor &discreteProbs) { } /* *******************************************************************************/ -AlgebraicDecisionTree GaussianMixture::logProbability( +AlgebraicDecisionTree HybridGaussianConditional::logProbability( const VectorValues &continuousValues) const { // functor to calculate (double) logProbability value from // GaussianConditional. @@ -331,7 +331,7 @@ AlgebraicDecisionTree GaussianMixture::logProbability( } /* ************************************************************************* */ -double GaussianMixture::conditionalError( +double HybridGaussianConditional::conditionalError( const GaussianConditional::shared_ptr &conditional, const VectorValues &continuousValues) const { // Check if valid pointer @@ -348,7 +348,7 @@ double GaussianMixture::conditionalError( } /* *******************************************************************************/ -AlgebraicDecisionTree GaussianMixture::errorTree( +AlgebraicDecisionTree HybridGaussianConditional::errorTree( const VectorValues &continuousValues) const { auto errorFunc = [&](const GaussianConditional::shared_ptr &conditional) { return conditionalError(conditional, continuousValues); @@ -358,20 +358,20 @@ AlgebraicDecisionTree GaussianMixture::errorTree( } /* *******************************************************************************/ -double GaussianMixture::error(const HybridValues &values) const { +double HybridGaussianConditional::error(const HybridValues &values) const { // Directly index to get the conditional, no need to build the whole tree. auto conditional = conditionals_(values.discrete()); return conditionalError(conditional, values.continuous()); } /* *******************************************************************************/ -double GaussianMixture::logProbability(const HybridValues &values) const { +double HybridGaussianConditional::logProbability(const HybridValues &values) const { auto conditional = conditionals_(values.discrete()); return conditional->logProbability(values.continuous()); } /* *******************************************************************************/ -double GaussianMixture::evaluate(const HybridValues &values) const { +double HybridGaussianConditional::evaluate(const HybridValues &values) const { auto conditional = conditionals_(values.discrete()); return conditional->evaluate(values.continuous()); } diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index 10feaf55b..87ddb2cb8 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file GaussianMixture.h + * @file HybridGaussianConditional.h * @brief A hybrid conditional in the Conditional Linear Gaussian scheme * @author Fan Jiang * @author Varun Agrawal @@ -50,14 +50,14 @@ class HybridValues; * * @ingroup hybrid */ -class GTSAM_EXPORT GaussianMixture +class GTSAM_EXPORT HybridGaussianConditional : public HybridFactor, - public Conditional { + public Conditional { public: - using This = GaussianMixture; - using shared_ptr = std::shared_ptr; + using This = HybridGaussianConditional; + using shared_ptr = std::shared_ptr; using BaseFactor = HybridFactor; - using BaseConditional = Conditional; + using BaseConditional = Conditional; /// typedef for Decision Tree of Gaussian Conditionals using Conditionals = DecisionTree; @@ -67,7 +67,7 @@ class GTSAM_EXPORT GaussianMixture double logConstant_; ///< log of the normalization constant. /** - * @brief Convert a GaussianMixture of conditionals into + * @brief Convert a HybridGaussianConditional of conditionals into * a DecisionTree of Gaussian factor graphs. */ GaussianFactorGraphTree asGaussianFactorGraphTree() const; @@ -88,10 +88,10 @@ class GTSAM_EXPORT GaussianMixture /// @{ /// Default constructor, mainly for serialization. - GaussianMixture() = default; + HybridGaussianConditional() = default; /** - * @brief Construct a new GaussianMixture object. + * @brief Construct a new HybridGaussianConditional object. * * @param continuousFrontals the continuous frontals. * @param continuousParents the continuous parents. @@ -101,7 +101,7 @@ class GTSAM_EXPORT GaussianMixture * cardinality of the DiscreteKeys in discreteParents, since the * discreteParents will be used as the labels in the decision tree. */ - GaussianMixture(const KeyVector &continuousFrontals, + HybridGaussianConditional(const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, const Conditionals &conditionals); @@ -114,7 +114,7 @@ class GTSAM_EXPORT GaussianMixture * @param discreteParents Discrete parents variables * @param conditionals List of conditionals */ - GaussianMixture(KeyVector &&continuousFrontals, KeyVector &&continuousParents, + HybridGaussianConditional(KeyVector &&continuousFrontals, KeyVector &&continuousParents, DiscreteKeys &&discreteParents, std::vector &&conditionals); @@ -126,7 +126,7 @@ class GTSAM_EXPORT GaussianMixture * @param discreteParents Discrete parents variables * @param conditionals List of conditionals */ - GaussianMixture( + HybridGaussianConditional( const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, const std::vector &conditionals); @@ -140,7 +140,7 @@ class GTSAM_EXPORT GaussianMixture /// Print utility void print( - const std::string &s = "GaussianMixture\n", + const std::string &s = "HybridGaussianConditional\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override; /// @} @@ -172,7 +172,7 @@ class GTSAM_EXPORT GaussianMixture const Conditionals &conditionals() const; /** - * @brief Compute logProbability of the GaussianMixture as a tree. + * @brief Compute logProbability of the HybridGaussianConditional as a tree. * * @param continuousValues The continuous VectorValues. * @return AlgebraicDecisionTree A decision tree with the same keys @@ -209,7 +209,7 @@ class GTSAM_EXPORT GaussianMixture double error(const HybridValues &values) const override; /** - * @brief Compute error of the GaussianMixture as a tree. + * @brief Compute error of the HybridGaussianConditional as a tree. * * @param continuousValues The continuous VectorValues. * @return AlgebraicDecisionTree A decision tree on the discrete keys @@ -277,6 +277,6 @@ std::set DiscreteKeysAsSet(const DiscreteKeys &discreteKeys); // traits template <> -struct traits : public Testable {}; +struct traits : public Testable {}; } // namespace gtsam diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 1d01baed2..90aa6cde2 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -168,11 +168,11 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) { DecisionTreeFactor prunedDiscreteProbs = this->pruneDiscreteConditionals(maxNrLeaves); - /* To prune, we visitWith every leaf in the GaussianMixture. + /* To prune, we visitWith every leaf in the HybridGaussianConditional. * For each leaf, using the assignment we can check the discrete decision tree * for 0.0 probability, then just set the leaf to a nullptr. * - * We can later check the GaussianMixture for just nullptrs. + * We can later check the HybridGaussianConditional for just nullptrs. */ HybridBayesNet prunedBayesNetFragment; @@ -182,14 +182,14 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) { for (auto &&conditional : *this) { if (auto gm = conditional->asMixture()) { // Make a copy of the Gaussian mixture and prune it! - auto prunedGaussianMixture = std::make_shared(*gm); + auto prunedGaussianMixture = std::make_shared(*gm); prunedGaussianMixture->prune(prunedDiscreteProbs); // imperative :-( // Type-erase and add to the pruned Bayes Net fragment. prunedBayesNetFragment.push_back(prunedGaussianMixture); } else { - // Add the non-GaussianMixture conditional + // Add the non-HybridGaussianConditional conditional prunedBayesNetFragment.push_back(conditional); } } diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 8fae4061d..5c453c106 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -79,7 +79,7 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { * * Example: * auto shared_ptr_to_a_conditional = - * std::make_shared(...); + * std::make_shared(...); * hbn.push_back(shared_ptr_to_a_conditional); */ void push_back(HybridConditional &&conditional) { @@ -106,7 +106,7 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { * Preferred: Emplace a conditional directly using arguments. * * Examples: - * hbn.emplace_shared(...))); + * hbn.emplace_shared(...))); * hbn.emplace_shared(...))); * hbn.emplace_shared(...))); */ diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index 6c4893476..5ef74ea7d 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -55,7 +55,7 @@ HybridConditional::HybridConditional( /* ************************************************************************ */ HybridConditional::HybridConditional( - const std::shared_ptr &gaussianMixture) + const std::shared_ptr &gaussianMixture) : BaseFactor(KeyVector(gaussianMixture->keys().begin(), gaussianMixture->keys().begin() + gaussianMixture->nrContinuous()), diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index fb6542822..d02b69127 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -18,7 +18,7 @@ #pragma once #include -#include +#include #include #include #include @@ -39,7 +39,7 @@ namespace gtsam { * As a type-erased variant of: * - DiscreteConditional * - GaussianConditional - * - GaussianMixture + * - HybridGaussianConditional * * The reason why this is important is that `Conditional` is a CRTP class. * CRTP is static polymorphism such that all CRTP classes, while bearing the @@ -127,7 +127,7 @@ class GTSAM_EXPORT HybridConditional * @param gaussianMixture Gaussian Mixture Conditional used to create the * HybridConditional. */ - HybridConditional(const std::shared_ptr& gaussianMixture); + HybridConditional(const std::shared_ptr& gaussianMixture); /// @} /// @name Testable @@ -146,12 +146,12 @@ class GTSAM_EXPORT HybridConditional /// @{ /** - * @brief Return HybridConditional as a GaussianMixture + * @brief Return HybridConditional as a HybridGaussianConditional * @return nullptr if not a mixture - * @return GaussianMixture::shared_ptr otherwise + * @return HybridGaussianConditional::shared_ptr otherwise */ - GaussianMixture::shared_ptr asMixture() const { - return std::dynamic_pointer_cast(inner_); + HybridGaussianConditional::shared_ptr asMixture() const { + return std::dynamic_pointer_cast(inner_); } /** @@ -222,8 +222,8 @@ class GTSAM_EXPORT HybridConditional boost::serialization::void_cast_register( static_cast(NULL), static_cast(NULL)); } else { - boost::serialization::void_cast_register( - static_cast(NULL), static_cast(NULL)); + boost::serialization::void_cast_register( + static_cast(NULL), static_cast(NULL)); } } #endif diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index 2a572ea65..c66116512 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -47,7 +47,7 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, * Examples: * - HybridNonlinearFactor * - HybridGaussianFactor - * - GaussianMixture + * - HybridGaussianConditional * * @ingroup hybrid */ diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 24669863e..656060ae6 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include #include #include @@ -180,7 +180,7 @@ GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const { result = addGaussian(result, gf); } else if (auto gmf = dynamic_pointer_cast(f)) { result = gmf->add(result); - } else if (auto gm = dynamic_pointer_cast(f)) { + } else if (auto gm = dynamic_pointer_cast(f)) { result = gm->add(result); } else if (auto hc = dynamic_pointer_cast(f)) { if (auto gm = hc->asMixture()) { @@ -408,10 +408,10 @@ hybridElimination(const HybridGaussianFactorGraph &factors, : createGaussianMixtureFactor(eliminationResults, continuousSeparator, discreteSeparator); - // Create the GaussianMixture from the conditionals - GaussianMixture::Conditionals conditionals( + // Create the HybridGaussianConditional from the conditionals + HybridGaussianConditional::Conditionals conditionals( eliminationResults, [](const Result &pair) { return pair.first; }); - auto gaussianMixture = std::make_shared( + auto gaussianMixture = std::make_shared( frontalKeys, continuousSeparator, discreteSeparator, conditionals); return {std::make_shared(gaussianMixture), newFactor}; @@ -458,7 +458,7 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors, // Because of all these reasons, we carefully consider how to // implement the hybrid factors so that we do not get poor performance. - // The first thing is how to represent the GaussianMixture. + // The first thing is how to represent the HybridGaussianConditional. // A very possible scenario is that the incoming factors will have different // levels of discrete keys. For example, imagine we are going to eliminate the // fragment: $\phi(x1,c1,c2)$, $\phi(x1,c2,c3)$, which is perfectly valid. @@ -599,7 +599,7 @@ GaussianFactorGraph HybridGaussianFactorGraph::operator()( gfg.push_back(gf); } else if (auto gmf = std::dynamic_pointer_cast(f)) { gfg.push_back((*gmf)(assignment)); - } else if (auto gm = dynamic_pointer_cast(f)) { + } else if (auto gm = dynamic_pointer_cast(f)) { gfg.push_back((*gm)(assignment)); } else { continue; diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp index 35a8ea355..76703ad2d 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include #include @@ -80,7 +80,7 @@ void HybridNonlinearFactorGraph::printErrors( gmf->errorTree(values.continuous()).print("", keyFormatter); std::cout << std::endl; } - } else if (auto gm = std::dynamic_pointer_cast(factor)) { + } else if (auto gm = std::dynamic_pointer_cast(factor)) { if (factor == nullptr) { std::cout << "nullptr" << "\n"; @@ -163,7 +163,7 @@ HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize( linearFG->push_back(f); } else if (auto gmf = dynamic_pointer_cast(f)) { linearFG->push_back(gmf); - } else if (auto gm = dynamic_pointer_cast(f)) { + } else if (auto gm = dynamic_pointer_cast(f)) { linearFG->push_back(gm); } else if (dynamic_pointer_cast(f)) { linearFG->push_back(f); diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp index afa8340d2..546837bf9 100644 --- a/gtsam/hybrid/HybridSmoother.cpp +++ b/gtsam/hybrid/HybridSmoother.cpp @@ -138,7 +138,7 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &originalGraph, } /* ************************************************************************* */ -GaussianMixture::shared_ptr HybridSmoother::gaussianMixture( +HybridGaussianConditional::shared_ptr HybridSmoother::gaussianMixture( size_t index) const { return hybridBayesNet_.at(index)->asMixture(); } diff --git a/gtsam/hybrid/HybridSmoother.h b/gtsam/hybrid/HybridSmoother.h index 5c2e4f546..fc40e0297 100644 --- a/gtsam/hybrid/HybridSmoother.h +++ b/gtsam/hybrid/HybridSmoother.h @@ -69,7 +69,7 @@ class GTSAM_EXPORT HybridSmoother { const HybridBayesNet& hybridBayesNet, const Ordering& ordering) const; /// Get the Gaussian Mixture from the Bayes Net posterior at `index`. - GaussianMixture::shared_ptr gaussianMixture(size_t index) const; + HybridGaussianConditional::shared_ptr gaussianMixture(size_t index) const; /// Return the Bayes Net posterior. const HybridBayesNet& hybridBayesNet() const; diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index e5be00c6f..834a067c3 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -65,7 +65,7 @@ virtual class HybridConditional { double logProbability(const gtsam::HybridValues& values) const; double evaluate(const gtsam::HybridValues& values) const; double operator()(const gtsam::HybridValues& values) const; - gtsam::GaussianMixture* asMixture() const; + gtsam::HybridGaussianConditional* asMixture() const; gtsam::GaussianConditional* asGaussian() const; gtsam::DiscreteConditional* asDiscrete() const; gtsam::Factor* inner(); @@ -84,9 +84,9 @@ class HybridGaussianFactor : gtsam::HybridFactor { gtsam::DefaultKeyFormatter) const; }; -#include -class GaussianMixture : gtsam::HybridFactor { - GaussianMixture(const gtsam::KeyVector& continuousFrontals, +#include +class HybridGaussianConditional : gtsam::HybridFactor { + HybridGaussianConditional(const gtsam::KeyVector& continuousFrontals, const gtsam::KeyVector& continuousParents, const gtsam::DiscreteKeys& discreteParents, const std::vector& @@ -97,7 +97,7 @@ class GaussianMixture : gtsam::HybridFactor { double logProbability(const gtsam::HybridValues& values) const; double evaluate(const gtsam::HybridValues& values) const; - void print(string s = "GaussianMixture\n", + void print(string s = "HybridGaussianConditional\n", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; }; @@ -131,7 +131,7 @@ class HybridBayesTree { #include class HybridBayesNet { HybridBayesNet(); - void push_back(const gtsam::GaussianMixture* s); + void push_back(const gtsam::HybridGaussianConditional* s); void push_back(const gtsam::GaussianConditional* s); void push_back(const gtsam::DiscreteConditional* s); diff --git a/gtsam/hybrid/tests/TinyHybridExample.h b/gtsam/hybrid/tests/TinyHybridExample.h index 26b83db29..00af28308 100644 --- a/gtsam/hybrid/tests/TinyHybridExample.h +++ b/gtsam/hybrid/tests/TinyHybridExample.h @@ -43,7 +43,7 @@ inline HybridBayesNet createHybridBayesNet(size_t num_measurements = 1, // Create Gaussian mixture z_i = x0 + noise for each measurement. for (size_t i = 0; i < num_measurements; i++) { const auto mode_i = manyModes ? DiscreteKey{M(i), 2} : mode; - bayesNet.emplace_shared( + bayesNet.emplace_shared( KeyVector{Z(i)}, KeyVector{X(0)}, DiscreteKeys{mode_i}, std::vector{GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 0.5), diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index a379dd036..116e1119d 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -11,7 +11,7 @@ /** * @file testGaussianMixture.cpp - * @brief Unit tests for GaussianMixture class + * @brief Unit tests for HybridGaussianConditional class * @author Varun Agrawal * @author Fan Jiang * @author Frank Dellaert @@ -19,7 +19,7 @@ */ #include -#include +#include #include #include #include @@ -46,19 +46,19 @@ static const HybridValues hv1{vv, assignment1}; /* ************************************************************************* */ namespace equal_constants { -// Create a simple GaussianMixture +// Create a simple HybridGaussianConditional const double commonSigma = 2.0; const std::vector conditionals{ GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), commonSigma), GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), commonSigma)}; -const GaussianMixture mixture({Z(0)}, {X(0)}, {mode}, conditionals); +const HybridGaussianConditional mixture({Z(0)}, {X(0)}, {mode}, conditionals); } // namespace equal_constants /* ************************************************************************* */ /// Check that invariants hold -TEST(GaussianMixture, Invariants) { +TEST(HybridGaussianConditional, Invariants) { using namespace equal_constants; // Check that the mixture normalization constant is the max of all constants @@ -67,13 +67,13 @@ TEST(GaussianMixture, Invariants) { EXPECT_DOUBLES_EQUAL(K, conditionals[0]->logNormalizationConstant(), 1e-8); EXPECT_DOUBLES_EQUAL(K, conditionals[1]->logNormalizationConstant(), 1e-8); - EXPECT(GaussianMixture::CheckInvariants(mixture, hv0)); - EXPECT(GaussianMixture::CheckInvariants(mixture, hv1)); + EXPECT(HybridGaussianConditional::CheckInvariants(mixture, hv0)); + EXPECT(HybridGaussianConditional::CheckInvariants(mixture, hv1)); } /* ************************************************************************* */ /// Check LogProbability. -TEST(GaussianMixture, LogProbability) { +TEST(HybridGaussianConditional, LogProbability) { using namespace equal_constants; auto actual = mixture.logProbability(vv); @@ -95,7 +95,7 @@ TEST(GaussianMixture, LogProbability) { /* ************************************************************************* */ /// Check error. -TEST(GaussianMixture, Error) { +TEST(HybridGaussianConditional, Error) { using namespace equal_constants; auto actual = mixture.errorTree(vv); @@ -118,7 +118,7 @@ TEST(GaussianMixture, Error) { /* ************************************************************************* */ /// Check that the likelihood is proportional to the conditional density given /// the measurements. -TEST(GaussianMixture, Likelihood) { +TEST(HybridGaussianConditional, Likelihood) { using namespace equal_constants; // Compute likelihood @@ -147,19 +147,19 @@ TEST(GaussianMixture, Likelihood) { /* ************************************************************************* */ namespace mode_dependent_constants { -// Create a GaussianMixture with mode-dependent noise models. +// Create a HybridGaussianConditional with mode-dependent noise models. // 0 is low-noise, 1 is high-noise. const std::vector conditionals{ GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), 0.5), GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), 3.0)}; -const GaussianMixture mixture({Z(0)}, {X(0)}, {mode}, conditionals); +const HybridGaussianConditional mixture({Z(0)}, {X(0)}, {mode}, conditionals); } // namespace mode_dependent_constants /* ************************************************************************* */ // Create a test for continuousParents. -TEST(GaussianMixture, ContinuousParents) { +TEST(HybridGaussianConditional, ContinuousParents) { using namespace mode_dependent_constants; const KeyVector continuousParentKeys = mixture.continuousParents(); // Check that the continuous parent keys are correct: @@ -170,7 +170,7 @@ TEST(GaussianMixture, ContinuousParents) { /* ************************************************************************* */ /// Check that the likelihood is proportional to the conditional density given /// the measurements. -TEST(GaussianMixture, Likelihood2) { +TEST(HybridGaussianConditional, Likelihood2) { using namespace mode_dependent_constants; // Compute likelihood diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index cd20e7215..b90950605 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include #include @@ -144,7 +144,7 @@ Hybrid [x1 x2; 1]{ } /* ************************************************************************* */ -TEST(HybridGaussianFactor, GaussianMixture) { +TEST(HybridGaussianFactor, HybridGaussianConditional) { KeyVector keys; keys.push_back(X(0)); keys.push_back(X(1)); @@ -154,8 +154,8 @@ TEST(HybridGaussianFactor, GaussianMixture) { dKeys.emplace_back(M(1), 2); auto gaussians = std::make_shared(); - GaussianMixture::Conditionals conditionals(gaussians); - GaussianMixture gm({}, keys, dKeys, conditionals); + HybridGaussianConditional::Conditionals conditionals(gaussians); + HybridGaussianConditional gm({}, keys, dKeys, conditionals); EXPECT_LONGS_EQUAL(2, gm.discreteKeys().size()); } @@ -229,7 +229,7 @@ static HybridBayesNet GetGaussianMixtureModel(double mu0, double mu1, c1 = make_shared(z, Vector1(mu1), I_1x1, model1); HybridBayesNet hbn; - hbn.emplace_shared(KeyVector{z}, KeyVector{}, + hbn.emplace_shared(KeyVector{z}, KeyVector{}, DiscreteKeys{m}, std::vector{c0, c1}); auto mixing = make_shared(m, "0.5/0.5"); @@ -413,7 +413,7 @@ static HybridBayesNet CreateBayesNet(double mu0, double mu1, double sigma0, c1 = make_shared(x1, Vector1(mu1), I_1x1, x0, -I_1x1, model1); - auto motion = std::make_shared( + auto motion = std::make_shared( KeyVector{x1}, KeyVector{x0}, DiscreteKeys{m1}, std::vector{c0, c1}); hbn.push_back(motion); diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 9b7ba9744..b35468302 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -107,7 +107,7 @@ TEST(HybridBayesNet, evaluateHybrid) { // Create hybrid Bayes net. HybridBayesNet bayesNet; bayesNet.push_back(continuousConditional); - bayesNet.emplace_shared( + bayesNet.emplace_shared( KeyVector{X(1)}, KeyVector{}, DiscreteKeys{Asia}, std::vector{conditional0, conditional1}); bayesNet.emplace_shared(Asia, "99/1"); @@ -168,7 +168,7 @@ TEST(HybridBayesNet, Error) { conditional1 = std::make_shared( X(1), Vector1::Constant(2), I_1x1, model1); - auto gm = std::make_shared( + auto gm = std::make_shared( KeyVector{X(1)}, KeyVector{}, DiscreteKeys{Asia}, std::vector{conditional0, conditional1}); // Create hybrid Bayes net. diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp index 406306df7..78dbd314c 100644 --- a/gtsam/hybrid/tests/testHybridConditional.cpp +++ b/gtsam/hybrid/tests/testHybridConditional.cpp @@ -43,9 +43,9 @@ TEST(HybridConditional, Invariants) { auto hc0 = bn.at(0); CHECK(hc0->isHybrid()); - // Check invariants as a GaussianMixture. + // Check invariants as a HybridGaussianConditional. const auto mixture = hc0->asMixture(); - EXPECT(GaussianMixture::CheckInvariants(*mixture, values)); + EXPECT(HybridGaussianConditional::CheckInvariants(*mixture, values)); // Check invariants as a HybridConditional. EXPECT(HybridConditional::CheckInvariants(*hc0, values)); diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 295633916..299562e32 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -616,7 +616,7 @@ TEST(HybridEstimation, ModeSelection) { GaussianConditional::sharedMeanAndStddev(Z(0), -I_1x1, X(0), Z_1x1, 0.1)); bn.push_back( GaussianConditional::sharedMeanAndStddev(Z(0), -I_1x1, X(1), Z_1x1, 0.1)); - bn.emplace_shared( + bn.emplace_shared( KeyVector{Z(0)}, KeyVector{X(0), X(1)}, DiscreteKeys{mode}, std::vector{GaussianConditional::sharedMeanAndStddev( Z(0), I_1x1, X(0), -I_1x1, X(1), Z_1x1, noise_loose), @@ -647,7 +647,7 @@ TEST(HybridEstimation, ModeSelection2) { GaussianConditional::sharedMeanAndStddev(Z(0), -I_3x3, X(0), Z_3x1, 0.1)); bn.push_back( GaussianConditional::sharedMeanAndStddev(Z(0), -I_3x3, X(1), Z_3x1, 0.1)); - bn.emplace_shared( + bn.emplace_shared( KeyVector{Z(0)}, KeyVector{X(0), X(1)}, DiscreteKeys{mode}, std::vector{GaussianConditional::sharedMeanAndStddev( Z(0), I_3x3, X(0), -I_3x3, X(1), Z_3x1, noise_loose), diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 96d0c478e..3386daac8 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include #include #include @@ -71,8 +71,8 @@ TEST(HybridGaussianFactorGraph, Creation) { // Define a gaussian mixture conditional P(x0|x1, c0) and add it to the factor // graph - GaussianMixture gm({X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{M(0), 2}), - GaussianMixture::Conditionals( + HybridGaussianConditional gm({X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{M(0), 2}), + HybridGaussianConditional::Conditionals( M(0), std::make_shared( X(0), Z_3x1, I_3x3, X(1), I_3x3), @@ -681,7 +681,7 @@ TEST(HybridGaussianFactorGraph, ErrorTreeWithConditional) { x0, -I_1x1, model0), c1 = make_shared(f01, Vector1(mu), I_1x1, x1, I_1x1, x0, -I_1x1, model1); - hbn.emplace_shared(KeyVector{f01}, KeyVector{x0, x1}, + hbn.emplace_shared(KeyVector{f01}, KeyVector{x0, x1}, DiscreteKeys{m1}, std::vector{c0, c1}); // Discrete uniform prior. @@ -805,7 +805,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1) { X(0), Vector1(14.1421), I_1x1 * 2.82843), conditional1 = std::make_shared( X(0), Vector1(10.1379), I_1x1 * 2.02759); - expectedBayesNet.emplace_shared( + expectedBayesNet.emplace_shared( KeyVector{X(0)}, KeyVector{}, DiscreteKeys{mode}, std::vector{conditional0, conditional1}); @@ -830,7 +830,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) { HybridBayesNet bn; // Create Gaussian mixture z_0 = x0 + noise for each measurement. - auto gm = std::make_shared( + auto gm = std::make_shared( KeyVector{Z(0)}, KeyVector{X(0)}, DiscreteKeys{mode}, std::vector{ GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Z_1x1, 3), @@ -862,7 +862,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) { X(0), Vector1(10.1379), I_1x1 * 2.02759), conditional1 = std::make_shared( X(0), Vector1(14.1421), I_1x1 * 2.82843); - expectedBayesNet.emplace_shared( + expectedBayesNet.emplace_shared( KeyVector{X(0)}, KeyVector{}, DiscreteKeys{mode}, std::vector{conditional0, conditional1}); @@ -899,7 +899,7 @@ TEST(HybridGaussianFactorGraph, EliminateTiny2) { X(0), Vector1(17.3205), I_1x1 * 3.4641), conditional1 = std::make_shared( X(0), Vector1(10.274), I_1x1 * 2.0548); - expectedBayesNet.emplace_shared( + expectedBayesNet.emplace_shared( KeyVector{X(0)}, KeyVector{}, DiscreteKeys{mode}, std::vector{conditional0, conditional1}); @@ -946,7 +946,7 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) { for (size_t t : {0, 1, 2}) { // Create Gaussian mixture on Z(t) conditioned on X(t) and mode N(t): const auto noise_mode_t = DiscreteKey{N(t), 2}; - bn.emplace_shared( + bn.emplace_shared( KeyVector{Z(t)}, KeyVector{X(t)}, DiscreteKeys{noise_mode_t}, std::vector{GaussianConditional::sharedMeanAndStddev(Z(t), I_1x1, X(t), Z_1x1, 0.5), @@ -961,7 +961,7 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) { for (size_t t : {2, 1}) { // Create Gaussian mixture on X(t) conditioned on X(t-1) and mode M(t-1): const auto motion_model_t = DiscreteKey{M(t), 2}; - auto gm = std::make_shared( + auto gm = std::make_shared( KeyVector{X(t)}, KeyVector{X(t - 1)}, DiscreteKeys{motion_model_t}, std::vector{GaussianConditional::sharedMeanAndStddev( X(t), I_1x1, X(t - 1), Z_1x1, 0.2), diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index d2b0fded3..8b15b50d2 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -134,22 +134,22 @@ TEST(HybridGaussianElimination, IncrementalInference) { // The densities on X(0) should be the same auto x0_conditional = - dynamic_pointer_cast(isam[X(0)]->conditional()->inner()); - auto expected_x0_conditional = dynamic_pointer_cast( + dynamic_pointer_cast(isam[X(0)]->conditional()->inner()); + auto expected_x0_conditional = dynamic_pointer_cast( (*expectedHybridBayesTree)[X(0)]->conditional()->inner()); EXPECT(assert_equal(*x0_conditional, *expected_x0_conditional)); // The densities on X(1) should be the same auto x1_conditional = - dynamic_pointer_cast(isam[X(1)]->conditional()->inner()); - auto expected_x1_conditional = dynamic_pointer_cast( + dynamic_pointer_cast(isam[X(1)]->conditional()->inner()); + auto expected_x1_conditional = dynamic_pointer_cast( (*expectedHybridBayesTree)[X(1)]->conditional()->inner()); EXPECT(assert_equal(*x1_conditional, *expected_x1_conditional)); // The densities on X(2) should be the same auto x2_conditional = - dynamic_pointer_cast(isam[X(2)]->conditional()->inner()); - auto expected_x2_conditional = dynamic_pointer_cast( + dynamic_pointer_cast(isam[X(2)]->conditional()->inner()); + auto expected_x2_conditional = dynamic_pointer_cast( (*expectedHybridBayesTree)[X(2)]->conditional()->inner()); EXPECT(assert_equal(*x2_conditional, *expected_x2_conditional)); @@ -279,9 +279,9 @@ TEST(HybridGaussianElimination, Approx_inference) { // Check that the hybrid nodes of the bayes net match those of the pre-pruning // bayes net, at the same positions. - auto &unprunedLastDensity = *dynamic_pointer_cast( + auto &unprunedLastDensity = *dynamic_pointer_cast( unprunedHybridBayesTree->clique(X(3))->conditional()->inner()); - auto &lastDensity = *dynamic_pointer_cast( + auto &lastDensity = *dynamic_pointer_cast( incrementalHybrid[X(3)]->conditional()->inner()); std::vector> assignments = diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 9e2816865..3f9a18653 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -350,7 +350,7 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) { EliminateHybrid(factors, ordering); auto gaussianConditionalMixture = - dynamic_pointer_cast(hybridConditionalMixture->inner()); + dynamic_pointer_cast(hybridConditionalMixture->inner()); CHECK(gaussianConditionalMixture); // Frontals = [x0, x1] diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index e2ed4cbad..06a4249d3 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -151,23 +151,23 @@ TEST(HybridNonlinearISAM, IncrementalInference) { .BaseEliminateable::eliminatePartialMultifrontal(ordering); // The densities on X(1) should be the same - auto x0_conditional = dynamic_pointer_cast( + auto x0_conditional = dynamic_pointer_cast( bayesTree[X(0)]->conditional()->inner()); - auto expected_x0_conditional = dynamic_pointer_cast( + auto expected_x0_conditional = dynamic_pointer_cast( (*expectedHybridBayesTree)[X(0)]->conditional()->inner()); EXPECT(assert_equal(*x0_conditional, *expected_x0_conditional)); // The densities on X(1) should be the same - auto x1_conditional = dynamic_pointer_cast( + auto x1_conditional = dynamic_pointer_cast( bayesTree[X(1)]->conditional()->inner()); - auto expected_x1_conditional = dynamic_pointer_cast( + auto expected_x1_conditional = dynamic_pointer_cast( (*expectedHybridBayesTree)[X(1)]->conditional()->inner()); EXPECT(assert_equal(*x1_conditional, *expected_x1_conditional)); // The densities on X(2) should be the same - auto x2_conditional = dynamic_pointer_cast( + auto x2_conditional = dynamic_pointer_cast( bayesTree[X(2)]->conditional()->inner()); - auto expected_x2_conditional = dynamic_pointer_cast( + auto expected_x2_conditional = dynamic_pointer_cast( (*expectedHybridBayesTree)[X(2)]->conditional()->inner()); EXPECT(assert_equal(*x2_conditional, *expected_x2_conditional)); @@ -300,9 +300,9 @@ TEST(HybridNonlinearISAM, Approx_inference) { // Check that the hybrid nodes of the bayes net match those of the pre-pruning // bayes net, at the same positions. - auto &unprunedLastDensity = *dynamic_pointer_cast( + auto &unprunedLastDensity = *dynamic_pointer_cast( unprunedHybridBayesTree->clique(X(3))->conditional()->inner()); - auto &lastDensity = *dynamic_pointer_cast( + auto &lastDensity = *dynamic_pointer_cast( bayesTree[X(3)]->conditional()->inner()); std::vector> assignments = diff --git a/gtsam/hybrid/tests/testSerializationHybrid.cpp b/gtsam/hybrid/tests/testSerializationHybrid.cpp index 31fe61d9b..17196d2ae 100644 --- a/gtsam/hybrid/tests/testSerializationHybrid.cpp +++ b/gtsam/hybrid/tests/testSerializationHybrid.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include #include @@ -59,12 +59,12 @@ BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::Factors::Leaf, BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::Factors::Choice, "gtsam_GaussianMixtureFactor_Factors_Choice"); -BOOST_CLASS_EXPORT_GUID(GaussianMixture, "gtsam_GaussianMixture"); -BOOST_CLASS_EXPORT_GUID(GaussianMixture::Conditionals, +BOOST_CLASS_EXPORT_GUID(HybridGaussianConditional, "gtsam_GaussianMixture"); +BOOST_CLASS_EXPORT_GUID(HybridGaussianConditional::Conditionals, "gtsam_GaussianMixture_Conditionals"); -BOOST_CLASS_EXPORT_GUID(GaussianMixture::Conditionals::Leaf, +BOOST_CLASS_EXPORT_GUID(HybridGaussianConditional::Conditionals::Leaf, "gtsam_GaussianMixture_Conditionals_Leaf"); -BOOST_CLASS_EXPORT_GUID(GaussianMixture::Conditionals::Choice, +BOOST_CLASS_EXPORT_GUID(HybridGaussianConditional::Conditionals::Choice, "gtsam_GaussianMixture_Conditionals_Choice"); // Needed since GaussianConditional::FromMeanAndStddev uses it BOOST_CLASS_EXPORT_GUID(noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); @@ -106,20 +106,20 @@ TEST(HybridSerialization, HybridConditional) { } /* ****************************************************************************/ -// Test GaussianMixture serialization. -TEST(HybridSerialization, GaussianMixture) { +// Test HybridGaussianConditional serialization. +TEST(HybridSerialization, HybridGaussianConditional) { const DiscreteKey mode(M(0), 2); Matrix1 I = Matrix1::Identity(); const auto conditional0 = std::make_shared( GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 0.5)); const auto conditional1 = std::make_shared( GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 3)); - const GaussianMixture gm({Z(0)}, {X(0)}, {mode}, + const HybridGaussianConditional gm({Z(0)}, {X(0)}, {mode}, {conditional0, conditional1}); - EXPECT(equalsObj(gm)); - EXPECT(equalsXML(gm)); - EXPECT(equalsBinary(gm)); + EXPECT(equalsObj(gm)); + EXPECT(equalsXML(gm)); + EXPECT(equalsBinary(gm)); } /* ****************************************************************************/ diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index 0ab8b49a4..c76c05ab1 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -46,7 +46,7 @@ namespace gtsam { * Gaussian density over a set of continuous variables. * - \b Discrete conditionals, implemented in \class DiscreteConditional, which * represent a discrete conditional distribution over discrete variables. - * - \b Hybrid conditional densities, such as \class GaussianMixture, which is + * - \b Hybrid conditional densities, such as \class HybridGaussianConditional, which is * a density over continuous variables given discrete/continuous parents. * - \b Symbolic factors, used to represent a graph structure, implemented in * \class SymbolicConditional. Only used for symbolic elimination etc. diff --git a/python/gtsam/tests/test_HybridBayesNet.py b/python/gtsam/tests/test_HybridBayesNet.py index bc685dec6..0fd569908 100644 --- a/python/gtsam/tests/test_HybridBayesNet.py +++ b/python/gtsam/tests/test_HybridBayesNet.py @@ -18,8 +18,9 @@ from gtsam.symbol_shorthand import A, X from gtsam.utils.test_case import GtsamTestCase from gtsam import (DiscreteConditional, DiscreteKeys, DiscreteValues, - GaussianConditional, GaussianMixture, HybridBayesNet, - HybridValues, VectorValues, noiseModel) + GaussianConditional, HybridBayesNet, + HybridGaussianConditional, HybridValues, VectorValues, + noiseModel) class TestHybridBayesNet(GtsamTestCase): @@ -49,7 +50,7 @@ class TestHybridBayesNet(GtsamTestCase): bayesNet = HybridBayesNet() bayesNet.push_back(conditional) bayesNet.push_back( - GaussianMixture([X(1)], [], discrete_keys, + HybridGaussianConditional([X(1)], [], discrete_keys, [conditional0, conditional1])) bayesNet.push_back(DiscreteConditional(Asia, "99/1")) diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py index 02a9b7b7d..c7c4c6b51 100644 --- a/python/gtsam/tests/test_HybridFactorGraph.py +++ b/python/gtsam/tests/test_HybridFactorGraph.py @@ -18,9 +18,9 @@ from gtsam.utils.test_case import GtsamTestCase import gtsam from gtsam import (DiscreteConditional, DiscreteKeys, GaussianConditional, - GaussianMixture, HybridBayesNet, HybridGaussianFactor, - HybridGaussianFactorGraph, HybridValues, JacobianFactor, - Ordering, noiseModel) + HybridBayesNet, HybridGaussianConditional, + HybridGaussianFactor, HybridGaussianFactorGraph, + HybridValues, JacobianFactor, Ordering, noiseModel) DEBUG_MARGINALS = False @@ -48,7 +48,7 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): self.assertEqual(hbn.size(), 2) mixture = hbn.at(0).inner() - self.assertIsInstance(mixture, GaussianMixture) + self.assertIsInstance(mixture, HybridGaussianConditional) self.assertEqual(len(mixture.keys()), 2) discrete_conditional = hbn.at(hbn.size() - 1).inner() @@ -106,7 +106,7 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): I_1x1, X(0), [0], sigma=3) - bayesNet.push_back(GaussianMixture([Z(i)], [X(0)], keys, + bayesNet.push_back(HybridGaussianConditional([Z(i)], [X(0)], keys, [conditional0, conditional1])) # Create prior on X(0).