diff --git a/gtsam/hybrid/GaussianMixtureConditional.cpp b/gtsam/hybrid/GaussianMixture.cpp similarity index 80% rename from gtsam/hybrid/GaussianMixtureConditional.cpp rename to gtsam/hybrid/GaussianMixture.cpp index 81d1244c2..000057518 100644 --- a/gtsam/hybrid/GaussianMixtureConditional.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file GaussianMixtureConditional.cpp + * @file GaussianMixture.cpp * @brief A hybrid conditional in the Conditional Linear Gaussian scheme * @author Fan Jiang * @author Varun Agrawal @@ -20,41 +20,41 @@ #include #include -#include +#include #include #include namespace gtsam { -GaussianMixtureConditional::GaussianMixtureConditional( +GaussianMixture::GaussianMixture( const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, - const GaussianMixtureConditional::Conditionals &conditionals) + const GaussianMixture::Conditionals &conditionals) : BaseFactor(CollectKeys(continuousFrontals, continuousParents), discreteParents), BaseConditional(continuousFrontals.size()), conditionals_(conditionals) {} /* *******************************************************************************/ -const GaussianMixtureConditional::Conditionals & -GaussianMixtureConditional::conditionals() { +const GaussianMixture::Conditionals & +GaussianMixture::conditionals() { return conditionals_; } /* *******************************************************************************/ -GaussianMixtureConditional GaussianMixtureConditional::FromConditionals( +GaussianMixture GaussianMixture::FromConditionals( const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, const std::vector &conditionalsList) { Conditionals dt(discreteParents, conditionalsList); - return GaussianMixtureConditional(continuousFrontals, continuousParents, + return GaussianMixture(continuousFrontals, continuousParents, discreteParents, dt); } /* *******************************************************************************/ -GaussianMixtureConditional::Sum GaussianMixtureConditional::add( - const GaussianMixtureConditional::Sum &sum) const { +GaussianMixture::Sum GaussianMixture::add( + const GaussianMixture::Sum &sum) const { using Y = GaussianFactorGraph; auto add = [](const Y &graph1, const Y &graph2) { auto result = graph1; @@ -66,8 +66,8 @@ GaussianMixtureConditional::Sum GaussianMixtureConditional::add( } /* *******************************************************************************/ -GaussianMixtureConditional::Sum -GaussianMixtureConditional::asGaussianFactorGraphTree() const { +GaussianMixture::Sum +GaussianMixture::asGaussianFactorGraphTree() const { auto lambda = [](const GaussianFactor::shared_ptr &factor) { GaussianFactorGraph result; result.push_back(factor); @@ -77,14 +77,14 @@ GaussianMixtureConditional::asGaussianFactorGraphTree() const { } /* *******************************************************************************/ -bool GaussianMixtureConditional::equals(const HybridFactor &lf, +bool GaussianMixture::equals(const HybridFactor &lf, double tol) const { const This *e = dynamic_cast(&lf); return e != nullptr && BaseFactor::equals(*e, tol); } /* *******************************************************************************/ -void GaussianMixtureConditional::print(const std::string &s, +void GaussianMixture::print(const std::string &s, const KeyFormatter &formatter) const { std::cout << s; if (isContinuous()) std::cout << "Continuous "; diff --git a/gtsam/hybrid/GaussianMixtureConditional.h b/gtsam/hybrid/GaussianMixture.h similarity index 76% rename from gtsam/hybrid/GaussianMixtureConditional.h rename to gtsam/hybrid/GaussianMixture.h index d12fa09d7..756e7b77b 100644 --- a/gtsam/hybrid/GaussianMixtureConditional.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file GaussianMixtureConditional.h + * @file GaussianMixture.h * @brief A hybrid conditional in the Conditional Linear Gaussian scheme * @author Fan Jiang * @author Varun Agrawal @@ -27,20 +27,26 @@ namespace gtsam { /** - * @brief A conditional of gaussian mixtures indexed by discrete variables. + * @brief A conditional of gaussian mixtures indexed by discrete variables, as + * part of a Bayes Network. * * Represents the conditional density P(X | M, Z) where X is a continuous random - * variable, M is the discrete variable and Z is the set of measurements. + * variable, M is the selection of discrete variables corresponding to a subset + * of the Gaussian variables and Z is parent of this node + * + * The negative log-probability is given by \f$ \sum_{m=1}^M \pi_m \frac{1}{2} + * |Rx - (d - Sy - Tz - ...)|^2 \f$, where \f$ \pi_m \f$ is the mixing + * coefficient. * */ -class GaussianMixtureConditional +class GTSAM_EXPORT GaussianMixture : public HybridFactor, - public Conditional { + public Conditional { public: - using This = GaussianMixtureConditional; - using shared_ptr = boost::shared_ptr; + using This = GaussianMixture; + using shared_ptr = boost::shared_ptr; using BaseFactor = HybridFactor; - using BaseConditional = Conditional; + using BaseConditional = Conditional; /// Alias for DecisionTree of GaussianFactorGraphs using Sum = DecisionTree; @@ -61,16 +67,20 @@ class GaussianMixtureConditional /// @{ /// Defaut constructor, mainly for serialization. - GaussianMixtureConditional() = default; + GaussianMixture() = default; + /** - * @brief Construct a new GaussianMixtureConditional object + * @brief Construct a new GaussianMixture object. * * @param continuousFrontals the continuous frontals. * @param continuousParents the continuous parents. * @param discreteParents the discrete parents. Will be placed last. - * @param conditionals a decision tree of GaussianConditionals. + * @param conditionals a decision tree of GaussianConditionals. The number of + * conditionals should be C^(number of discrete parents), where C is the + * cardinality of the DiscreteKeys in discreteParents, since the + * discreteParents will be used as the labels in the decision tree. */ - GaussianMixtureConditional(const KeyVector &continuousFrontals, + GaussianMixture(const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, const Conditionals &conditionals); @@ -97,7 +107,7 @@ class GaussianMixtureConditional /* print utility */ void print( - const std::string &s = "GaussianMixtureConditional\n", + const std::string &s = "GaussianMixture\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override; /// @} diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index c8d70e67e..ff07f1817 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -11,7 +11,8 @@ /** * @file HybridBayesTree.cpp - * @brief Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree + * @brief Hybrid Bayes Tree, the result of eliminating a + * HybridGaussianJunctionTree * @date Mar 11, 2022 * @author Fan Jiang */ @@ -25,7 +26,8 @@ namespace gtsam { // Instantiate base class -template class BayesTreeCliqueBase; +template class BayesTreeCliqueBase; template class BayesTree; /* ************************************************************************* */ diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index f8a90d6b4..28d9ef809 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -11,7 +11,8 @@ /** * @file HybridBayesTree.h - * @brief Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree + * @brief Hybrid Bayes Tree, the result of eliminating a + * HybridGaussianJunctionTree * @date Mar 11, 2022 * @author Fan Jiang */ @@ -19,7 +20,7 @@ #pragma once #include -#include +#include #include #include #include @@ -37,10 +38,12 @@ class VectorValues; * which is a HybridConditional internally. */ class GTSAM_EXPORT HybridBayesTreeClique - : public BayesTreeCliqueBase { + : public BayesTreeCliqueBase { public: typedef HybridBayesTreeClique This; - typedef BayesTreeCliqueBase Base; + typedef BayesTreeCliqueBase + Base; typedef boost::shared_ptr shared_ptr; typedef boost::weak_ptr weak_ptr; HybridBayesTreeClique() {} diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index e3932308b..8e071532d 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -54,7 +54,7 @@ HybridConditional::HybridConditional( /* ************************************************************************ */ HybridConditional::HybridConditional( - boost::shared_ptr gaussianMixture) + boost::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 b942773cb..3ba5da393 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -18,9 +18,9 @@ #pragma once #include -#include +#include #include -#include +#include #include #include #include @@ -34,7 +34,7 @@ namespace gtsam { -class HybridFactorGraph; +class HybridGaussianFactorGraph; /** * Hybrid Conditional Density @@ -42,7 +42,7 @@ class HybridFactorGraph; * As a type-erased variant of: * - DiscreteConditional * - GaussianConditional - * - GaussianMixtureConditional + * - GaussianMixture * * 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 @@ -128,16 +128,16 @@ class GTSAM_EXPORT HybridConditional * HybridConditional. */ HybridConditional( - boost::shared_ptr gaussianMixture); + boost::shared_ptr gaussianMixture); /** - * @brief Return HybridConditional as a GaussianMixtureConditional + * @brief Return HybridConditional as a GaussianMixture * - * @return GaussianMixtureConditional::shared_ptr + * @return GaussianMixture::shared_ptr */ - GaussianMixtureConditional::shared_ptr asMixture() { + GaussianMixture::shared_ptr asMixture() { if (!isHybrid()) throw std::invalid_argument("Not a mixture"); - return boost::static_pointer_cast(inner_); + return boost::static_pointer_cast(inner_); } /** @@ -146,7 +146,8 @@ class GTSAM_EXPORT HybridConditional * @return DiscreteConditional::shared_ptr */ DiscreteConditional::shared_ptr asDiscreteConditional() { - if (!isDiscrete()) throw std::invalid_argument("Not a discrete conditional"); + if (!isDiscrete()) + throw std::invalid_argument("Not a discrete conditional"); return boost::static_pointer_cast(inner_); } diff --git a/gtsam/hybrid/HybridEliminationTree.cpp b/gtsam/hybrid/HybridEliminationTree.cpp index ecac96724..c2df2dd60 100644 --- a/gtsam/hybrid/HybridEliminationTree.cpp +++ b/gtsam/hybrid/HybridEliminationTree.cpp @@ -21,17 +21,17 @@ namespace gtsam { // Instantiate base class -template class EliminationTree; +template class EliminationTree; /* ************************************************************************* */ HybridEliminationTree::HybridEliminationTree( - const HybridFactorGraph& factorGraph, const VariableIndex& structure, - const Ordering& order) + const HybridGaussianFactorGraph& factorGraph, + const VariableIndex& structure, const Ordering& order) : Base(factorGraph, structure, order) {} /* ************************************************************************* */ HybridEliminationTree::HybridEliminationTree( - const HybridFactorGraph& factorGraph, const Ordering& order) + const HybridGaussianFactorGraph& factorGraph, const Ordering& order) : Base(factorGraph, order) {} /* ************************************************************************* */ diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h index 27766724a..77a84fea8 100644 --- a/gtsam/hybrid/HybridEliminationTree.h +++ b/gtsam/hybrid/HybridEliminationTree.h @@ -18,7 +18,7 @@ #pragma once #include -#include +#include #include namespace gtsam { @@ -27,12 +27,12 @@ namespace gtsam { * Elimination Tree type for Hybrid */ class GTSAM_EXPORT HybridEliminationTree - : public EliminationTree { + : public EliminationTree { private: friend class ::EliminationTreeTester; public: - typedef EliminationTree + typedef EliminationTree Base; ///< Base class typedef HybridEliminationTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class @@ -49,7 +49,7 @@ class GTSAM_EXPORT HybridEliminationTree * named constructor instead. * @return The elimination tree */ - HybridEliminationTree(const HybridFactorGraph& factorGraph, + HybridEliminationTree(const HybridGaussianFactorGraph& factorGraph, const VariableIndex& structure, const Ordering& order); /** Build the elimination tree of a factor graph. Note that this has to @@ -57,7 +57,7 @@ class GTSAM_EXPORT HybridEliminationTree * this precomputed, use the other constructor instead. * @param factorGraph The factor graph for which to build the elimination tree */ - HybridEliminationTree(const HybridFactorGraph& factorGraph, + HybridEliminationTree(const HybridGaussianFactorGraph& factorGraph, const Ordering& order); /// @} diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp similarity index 92% rename from gtsam/hybrid/HybridFactorGraph.cpp rename to gtsam/hybrid/HybridGaussianFactorGraph.cpp index dbb4a62b3..f4f09701f 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridFactorGraph.cpp + * @file HybridGaussianFactorGraph.cpp * @brief Hybrid factor graph that uses type erasure * @author Fan Jiang * @author Varun Agrawal @@ -23,15 +23,15 @@ #include #include #include -#include +#include #include #include #include #include #include -#include #include -#include +#include +#include #include #include #include @@ -53,7 +53,7 @@ namespace gtsam { -template class EliminateableFactorGraph; +template class EliminateableFactorGraph; /* ************************************************************************ */ static GaussianMixtureFactor::Sum &addGaussian( @@ -78,7 +78,7 @@ static GaussianMixtureFactor::Sum &addGaussian( /* ************************************************************************ */ std::pair -continuousElimination(const HybridFactorGraph &factors, +continuousElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) { GaussianFactorGraph gfg; for (auto &fp : factors) { @@ -103,7 +103,7 @@ continuousElimination(const HybridFactorGraph &factors, /* ************************************************************************ */ std::pair -discreteElimination(const HybridFactorGraph &factors, +discreteElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) { DiscreteFactorGraph dfg; for (auto &fp : factors) { @@ -129,7 +129,8 @@ discreteElimination(const HybridFactorGraph &factors, /* ************************************************************************ */ std::pair -hybridElimination(const HybridFactorGraph &factors, const Ordering &frontalKeys, +hybridElimination(const HybridGaussianFactorGraph &factors, + const Ordering &frontalKeys, const KeySet &continuousSeparator, const std::set &discreteSeparatorSet) { // NOTE: since we use the special JunctionTree, @@ -206,8 +207,8 @@ hybridElimination(const HybridFactorGraph &factors, const Ordering &frontalKeys, const GaussianMixtureFactor::Factors &separatorFactors = pair.second; - // Create the GaussianMixtureConditional from the conditionals - auto conditional = boost::make_shared( + // Create the GaussianMixture from the conditionals + auto conditional = boost::make_shared( frontalKeys, keysOfSeparator, discreteSeparator, pair.first); // If there are no more continuous parents, then we should create here a @@ -235,7 +236,8 @@ hybridElimination(const HybridFactorGraph &factors, const Ordering &frontalKeys, } /* ************************************************************************ */ std::pair // -EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { +EliminateHybrid(const HybridGaussianFactorGraph &factors, + const Ordering &frontalKeys) { // NOTE: Because we are in the Conditional Gaussian regime there are only // a few cases: // 1. continuous variable, make a Gaussian Mixture if there are hybrid @@ -260,7 +262,7 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { // 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 GaussianMixtureConditional. + // The first thing is how to represent the GaussianMixture. // 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. @@ -343,22 +345,22 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { } /* ************************************************************************ */ -void HybridFactorGraph::add(JacobianFactor &&factor) { +void HybridGaussianFactorGraph::add(JacobianFactor &&factor) { FactorGraph::add(boost::make_shared(std::move(factor))); } /* ************************************************************************ */ -void HybridFactorGraph::add(JacobianFactor::shared_ptr factor) { +void HybridGaussianFactorGraph::add(JacobianFactor::shared_ptr factor) { FactorGraph::add(boost::make_shared(factor)); } /* ************************************************************************ */ -void HybridFactorGraph::add(DecisionTreeFactor &&factor) { +void HybridGaussianFactorGraph::add(DecisionTreeFactor &&factor) { FactorGraph::add(boost::make_shared(std::move(factor))); } /* ************************************************************************ */ -void HybridFactorGraph::add(DecisionTreeFactor::shared_ptr factor) { +void HybridGaussianFactorGraph::add(DecisionTreeFactor::shared_ptr factor) { FactorGraph::add(boost::make_shared(factor)); } diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h similarity index 68% rename from gtsam/hybrid/HybridFactorGraph.h rename to gtsam/hybrid/HybridGaussianFactorGraph.h index 10670b6b7..6944af510 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridFactorGraph.h - * @brief Hybrid factor graph that uses type erasure + * @file HybridGaussianFactorGraph.h + * @brief Linearized Hybrid factor graph that uses type erasure * @author Fan Jiang * @date Mar 11, 2022 */ @@ -26,35 +26,37 @@ namespace gtsam { // Forward declarations -class HybridFactorGraph; +class HybridGaussianFactorGraph; class HybridConditional; class HybridBayesNet; class HybridEliminationTree; class HybridBayesTree; -class HybridJunctionTree; +class HybridGaussianJunctionTree; class DecisionTreeFactor; class JacobianFactor; -/** Main elimination function for HybridFactorGraph */ +/** Main elimination function for HybridGaussianFactorGraph */ GTSAM_EXPORT std::pair, HybridFactor::shared_ptr> -EliminateHybrid(const HybridFactorGraph& factors, const Ordering& keys); +EliminateHybrid(const HybridGaussianFactorGraph& factors, const Ordering& keys); /* ************************************************************************* */ template <> -struct EliminationTraits { +struct EliminationTraits { typedef HybridFactor FactorType; ///< Type of factors in factor graph - typedef HybridFactorGraph - FactorGraphType; ///< Type of the factor graph (e.g. HybridFactorGraph) + typedef HybridGaussianFactorGraph + FactorGraphType; ///< Type of the factor graph (e.g. + ///< HybridGaussianFactorGraph) typedef HybridConditional ConditionalType; ///< Type of conditionals from elimination typedef HybridBayesNet BayesNetType; ///< Type of Bayes net from sequential elimination typedef HybridEliminationTree - EliminationTreeType; ///< Type of elimination tree - typedef HybridBayesTree BayesTreeType; ///< Type of Bayes tree - typedef HybridJunctionTree JunctionTreeType; ///< Type of Junction tree + EliminationTreeType; ///< Type of elimination tree + typedef HybridBayesTree BayesTreeType; ///< Type of Bayes tree + typedef HybridGaussianJunctionTree + JunctionTreeType; ///< Type of Junction tree /// The default dense elimination function static std::pair, boost::shared_ptr > @@ -64,16 +66,17 @@ struct EliminationTraits { }; /** - * Hybrid Factor Graph + * Gaussian Hybrid Factor Graph * ----------------------- - * This is the linear version of a hybrid factor graph. Everything inside needs - * to be hybrid factor or hybrid conditional. + * This is the linearized version of a hybrid factor graph. + * Everything inside needs to be hybrid factor or hybrid conditional. */ -class HybridFactorGraph : public FactorGraph, - public EliminateableFactorGraph { +class HybridGaussianFactorGraph + : public FactorGraph, + public EliminateableFactorGraph { public: using Base = FactorGraph; - using This = HybridFactorGraph; ///< this class + using This = HybridGaussianFactorGraph; ///< this class using BaseEliminateable = EliminateableFactorGraph; ///< for elimination using shared_ptr = boost::shared_ptr; ///< shared_ptr to This @@ -84,7 +87,7 @@ class HybridFactorGraph : public FactorGraph, /// @name Constructors /// @{ - HybridFactorGraph() = default; + HybridGaussianFactorGraph() = default; /** * Implicit copy/downcast constructor to override explicit template container @@ -92,7 +95,8 @@ class HybridFactorGraph : public FactorGraph, * `cachedSeparatorMarginal_.reset(*separatorMarginal)` * */ template - HybridFactorGraph(const FactorGraph& graph) : Base(graph) {} + HybridGaussianFactorGraph(const FactorGraph& graph) + : Base(graph) {} /// @} diff --git a/gtsam/hybrid/HybridISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp similarity index 84% rename from gtsam/hybrid/HybridISAM.cpp rename to gtsam/hybrid/HybridGaussianISAM.cpp index 057f784ad..7783a88dd 100644 --- a/gtsam/hybrid/HybridISAM.cpp +++ b/gtsam/hybrid/HybridGaussianISAM.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridISAM.h + * @file HybridGaussianISAM.h * @date March 31, 2022 * @author Fan Jiang * @author Frank Dellaert @@ -18,8 +18,8 @@ */ #include -#include -#include +#include +#include #include #include @@ -31,15 +31,17 @@ namespace gtsam { // template class ISAM; /* ************************************************************************* */ -HybridISAM::HybridISAM() {} +HybridGaussianISAM::HybridGaussianISAM() {} /* ************************************************************************* */ -HybridISAM::HybridISAM(const HybridBayesTree& bayesTree) : Base(bayesTree) {} +HybridGaussianISAM::HybridGaussianISAM(const HybridBayesTree& bayesTree) + : Base(bayesTree) {} /* ************************************************************************* */ -void HybridISAM::updateInternal(const HybridFactorGraph& newFactors, - HybridBayesTree::Cliques* orphans, - const HybridBayesTree::Eliminate& function) { +void HybridGaussianISAM::updateInternal( + const HybridGaussianFactorGraph& newFactors, + HybridBayesTree::Cliques* orphans, + const HybridBayesTree::Eliminate& function) { // Remove the contaminated part of the Bayes tree BayesNetType bn; const KeySet newFactorKeys = newFactors.keys(); @@ -90,8 +92,8 @@ void HybridISAM::updateInternal(const HybridFactorGraph& newFactors, } /* ************************************************************************* */ -void HybridISAM::update(const HybridFactorGraph& newFactors, - const HybridBayesTree::Eliminate& function) { +void HybridGaussianISAM::update(const HybridGaussianFactorGraph& newFactors, + const HybridBayesTree::Eliminate& function) { Cliques orphans; this->updateInternal(newFactors, &orphans, function); } diff --git a/gtsam/hybrid/HybridISAM.h b/gtsam/hybrid/HybridGaussianISAM.h similarity index 74% rename from gtsam/hybrid/HybridISAM.h rename to gtsam/hybrid/HybridGaussianISAM.h index 0738f46c8..d5b6271da 100644 --- a/gtsam/hybrid/HybridISAM.h +++ b/gtsam/hybrid/HybridGaussianISAM.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridISAM.h + * @file HybridGaussianISAM.h * @date March 31, 2022 * @author Fan Jiang * @author Frank Dellaert @@ -21,32 +21,33 @@ #include #include -#include +#include #include namespace gtsam { -class GTSAM_EXPORT HybridISAM : public ISAM { +class GTSAM_EXPORT HybridGaussianISAM : public ISAM { public: typedef ISAM Base; - typedef HybridISAM This; + typedef HybridGaussianISAM This; typedef boost::shared_ptr shared_ptr; /// @name Standard Constructors /// @{ /** Create an empty Bayes Tree */ - HybridISAM(); + HybridGaussianISAM(); /** Copy constructor */ - HybridISAM(const HybridBayesTree& bayesTree); + HybridGaussianISAM(const HybridBayesTree& bayesTree); /// @} private: /// Internal method that performs the ISAM update. void updateInternal( - const HybridFactorGraph& newFactors, HybridBayesTree::Cliques* orphans, + const HybridGaussianFactorGraph& newFactors, + HybridBayesTree::Cliques* orphans, const HybridBayesTree::Eliminate& function = HybridBayesTree::EliminationTraitsType::DefaultEliminate); @@ -57,13 +58,13 @@ class GTSAM_EXPORT HybridISAM : public ISAM { * @param newFactors Factor graph of new factors to add and eliminate. * @param function Elimination function. */ - void update(const HybridFactorGraph& newFactors, + void update(const HybridGaussianFactorGraph& newFactors, const HybridBayesTree::Eliminate& function = HybridBayesTree::EliminationTraitsType::DefaultEliminate); }; /// traits template <> -struct traits : public Testable {}; +struct traits : public Testable {}; } // namespace gtsam diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridGaussianJunctionTree.cpp similarity index 70% rename from gtsam/hybrid/HybridJunctionTree.cpp rename to gtsam/hybrid/HybridGaussianJunctionTree.cpp index 981cd6f32..8ceb7c87b 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridGaussianJunctionTree.cpp @@ -10,14 +10,14 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridJunctionTree.cpp + * @file HybridGaussianJunctionTree.cpp * @date Mar 11, 2022 * @author Fan Jiang */ #include -#include -#include +#include +#include #include #include @@ -26,16 +26,20 @@ namespace gtsam { // Instantiate base classes -template class EliminatableClusterTree; -template class JunctionTree; +template class EliminatableClusterTree; +template class JunctionTree; struct HybridConstructorTraversalData { - typedef typename JunctionTree::Node Node; - typedef typename JunctionTree::sharedNode - sharedNode; + typedef + typename JunctionTree::Node + Node; + typedef + typename JunctionTree::sharedNode sharedNode; HybridConstructorTraversalData* const parentData; - sharedNode myJTNode; + sharedNode junctionTreeNode; FastVector childSymbolicConditionals; FastVector childSymbolicFactors; KeySet discreteKeys; @@ -53,24 +57,24 @@ struct HybridConstructorTraversalData { // On the pre-order pass, before children have been visited, we just set up // a traversal data structure with its own JT node, and create a child // pointer in its parent. - HybridConstructorTraversalData myData = + HybridConstructorTraversalData data = HybridConstructorTraversalData(&parentData); - myData.myJTNode = boost::make_shared(node->key, node->factors); - parentData.myJTNode->addChild(myData.myJTNode); + data.junctionTreeNode = boost::make_shared(node->key, node->factors); + parentData.junctionTreeNode->addChild(data.junctionTreeNode); for (HybridFactor::shared_ptr& f : node->factors) { for (auto& k : f->discreteKeys()) { - myData.discreteKeys.insert(k.first); + data.discreteKeys.insert(k.first); } } - return myData; + return data; } // Post-order visitor function static void ConstructorTraversalVisitorPostAlg2( const boost::shared_ptr& ETreeNode, - const HybridConstructorTraversalData& myData) { + const HybridConstructorTraversalData& data) { // In this post-order visitor, we combine the symbolic elimination results // from the elimination tree children and symbolically eliminate the current // elimination tree node. We then check whether each of our elimination @@ -83,50 +87,50 @@ struct HybridConstructorTraversalData { // Do symbolic elimination for this node SymbolicFactors symbolicFactors; symbolicFactors.reserve(ETreeNode->factors.size() + - myData.childSymbolicFactors.size()); + data.childSymbolicFactors.size()); // Add ETree node factors symbolicFactors += ETreeNode->factors; // Add symbolic factors passed up from children - symbolicFactors += myData.childSymbolicFactors; + symbolicFactors += data.childSymbolicFactors; Ordering keyAsOrdering; keyAsOrdering.push_back(ETreeNode->key); - SymbolicConditional::shared_ptr myConditional; - SymbolicFactor::shared_ptr mySeparatorFactor; - boost::tie(myConditional, mySeparatorFactor) = + SymbolicConditional::shared_ptr conditional; + SymbolicFactor::shared_ptr separatorFactor; + boost::tie(conditional, separatorFactor) = internal::EliminateSymbolic(symbolicFactors, keyAsOrdering); // Store symbolic elimination results in the parent - myData.parentData->childSymbolicConditionals.push_back(myConditional); - myData.parentData->childSymbolicFactors.push_back(mySeparatorFactor); - myData.parentData->discreteKeys.merge(myData.discreteKeys); + data.parentData->childSymbolicConditionals.push_back(conditional); + data.parentData->childSymbolicFactors.push_back(separatorFactor); + data.parentData->discreteKeys.merge(data.discreteKeys); - sharedNode node = myData.myJTNode; + sharedNode node = data.junctionTreeNode; const FastVector& childConditionals = - myData.childSymbolicConditionals; - node->problemSize_ = (int)(myConditional->size() * symbolicFactors.size()); + data.childSymbolicConditionals; + node->problemSize_ = (int)(conditional->size() * symbolicFactors.size()); // Merge our children if they are in our clique - if our conditional has // exactly one fewer parent than our child's conditional. - const size_t myNrParents = myConditional->nrParents(); + const size_t nrParents = conditional->nrParents(); const size_t nrChildren = node->nrChildren(); assert(childConditionals.size() == nrChildren); // decide which children to merge, as index into children - std::vector nrFrontals = node->nrFrontalsOfChildren(); + std::vector nrChildrenFrontals = node->nrFrontalsOfChildren(); std::vector merge(nrChildren, false); - size_t myNrFrontals = 1; + size_t nrFrontals = 1; for (size_t i = 0; i < nrChildren; i++) { // Check if we should merge the i^th child - if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) { + if (nrParents + nrFrontals == childConditionals[i]->nrParents()) { const bool myType = - myData.discreteKeys.exists(myConditional->frontals()[0]); + data.discreteKeys.exists(conditional->frontals()[0]); const bool theirType = - myData.discreteKeys.exists(childConditionals[i]->frontals()[0]); + data.discreteKeys.exists(childConditionals[i]->frontals()[0]); if (myType == theirType) { // Increment number of frontal variables - myNrFrontals += nrFrontals[i]; + nrFrontals += nrChildrenFrontals[i]; merge[i] = true; } } @@ -138,7 +142,7 @@ struct HybridConstructorTraversalData { }; /* ************************************************************************* */ -HybridJunctionTree::HybridJunctionTree( +HybridGaussianJunctionTree::HybridGaussianJunctionTree( const HybridEliminationTree& eliminationTree) { gttic(JunctionTree_FromEliminationTree); // Here we rely on the BayesNet having been produced by this elimination tree, @@ -152,7 +156,7 @@ HybridJunctionTree::HybridJunctionTree( // as we go. Gather the created junction tree roots in a dummy Node. typedef HybridConstructorTraversalData Data; Data rootData(0); - rootData.myJTNode = + rootData.junctionTreeNode = boost::make_shared(); // Make a dummy node to gather // the junction tree roots treeTraversal::DepthFirstForest(eliminationTree, rootData, @@ -160,7 +164,7 @@ HybridJunctionTree::HybridJunctionTree( Data::ConstructorTraversalVisitorPostAlg2); // Assign roots from the dummy node - this->addChildrenAsRoots(rootData.myJTNode); + this->addChildrenAsRoots(rootData.junctionTreeNode); // Transfer remaining factors from elimination tree Base::remainingFactors_ = eliminationTree.remainingFactors(); diff --git a/gtsam/hybrid/HybridJunctionTree.h b/gtsam/hybrid/HybridGaussianJunctionTree.h similarity index 84% rename from gtsam/hybrid/HybridJunctionTree.h rename to gtsam/hybrid/HybridGaussianJunctionTree.h index 824fa4f85..314e7daa6 100644 --- a/gtsam/hybrid/HybridJunctionTree.h +++ b/gtsam/hybrid/HybridGaussianJunctionTree.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridJunctionTree.h + * @file HybridGaussianJunctionTree.h * @date Mar 11, 2022 * @author Fan Jiang */ @@ -18,7 +18,7 @@ #pragma once #include -#include +#include #include namespace gtsam { @@ -48,12 +48,12 @@ class HybridEliminationTree; * \addtogroup Multifrontal * \nosubgrouping */ -class GTSAM_EXPORT HybridJunctionTree - : public JunctionTree { +class GTSAM_EXPORT HybridGaussianJunctionTree + : public JunctionTree { public: - typedef JunctionTree + typedef JunctionTree Base; ///< Base class - typedef HybridJunctionTree This; ///< This class + typedef HybridGaussianJunctionTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class /** @@ -65,7 +65,7 @@ class GTSAM_EXPORT HybridJunctionTree * named constructor instead. * @return The elimination tree */ - HybridJunctionTree(const HybridEliminationTree& eliminationTree); + HybridGaussianJunctionTree(const HybridEliminationTree& eliminationTree); }; } // namespace gtsam diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index d3ff98719..bbe1e2400 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -38,16 +38,16 @@ class GaussianMixtureFactor : gtsam::HybridFactor { gtsam::DefaultKeyFormatter) const; }; -#include -class GaussianMixtureConditional : gtsam::HybridFactor { - static GaussianMixtureConditional FromConditionals( +#include +class GaussianMixture : gtsam::HybridFactor { + static GaussianMixture FromConditionals( const gtsam::KeyVector& continuousFrontals, const gtsam::KeyVector& continuousParents, const gtsam::DiscreteKeys& discreteParents, const std::vector& conditionalsList); - void print(string s = "GaussianMixtureConditional\n", + void print(string s = "GaussianMixture\n", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; }; @@ -98,15 +98,15 @@ class HybridBayesNet { const gtsam::DotWriter& writer = gtsam::DotWriter()) const; }; -#include -class HybridFactorGraph { - HybridFactorGraph(); - HybridFactorGraph(const gtsam::HybridBayesNet& bayesNet); +#include +class HybridGaussianFactorGraph { + HybridGaussianFactorGraph(); + HybridGaussianFactorGraph(const gtsam::HybridBayesNet& bayesNet); // Building the graph void push_back(const gtsam::HybridFactor* factor); void push_back(const gtsam::HybridConditional* conditional); - void push_back(const gtsam::HybridFactorGraph& graph); + void push_back(const gtsam::HybridGaussianFactorGraph& graph); void push_back(const gtsam::HybridBayesNet& bayesNet); void push_back(const gtsam::HybridBayesTree& bayesTree); void push_back(const gtsam::GaussianMixtureFactor* gmm); @@ -120,13 +120,13 @@ class HybridFactorGraph { const gtsam::HybridFactor* at(size_t i) const; void print(string s = "") const; - bool equals(const gtsam::HybridFactorGraph& fg, double tol = 1e-9) const; + bool equals(const gtsam::HybridGaussianFactorGraph& fg, double tol = 1e-9) const; gtsam::HybridBayesNet* eliminateSequential(); gtsam::HybridBayesNet* eliminateSequential( gtsam::Ordering::OrderingType type); gtsam::HybridBayesNet* eliminateSequential(const gtsam::Ordering& ordering); - pair + pair eliminatePartialSequential(const gtsam::Ordering& ordering); gtsam::HybridBayesTree* eliminateMultifrontal(); @@ -134,7 +134,7 @@ class HybridFactorGraph { gtsam::Ordering::OrderingType type); gtsam::HybridBayesTree* eliminateMultifrontal( const gtsam::Ordering& ordering); - pair + pair eliminatePartialMultifrontal(const gtsam::Ordering& ordering); string dot( diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 1a4db7898..c081b8e87 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -19,7 +19,7 @@ #include #include #include -#include +#include #include #include @@ -29,10 +29,10 @@ using gtsam::symbol_shorthand::C; using gtsam::symbol_shorthand::X; namespace gtsam { -inline HybridFactorGraph::shared_ptr makeSwitchingChain( +inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain( size_t n, std::function keyFunc = X, std::function dKeyFunc = C) { - HybridFactorGraph hfg; + HybridGaussianFactorGraph hfg; hfg.add(JacobianFactor(keyFunc(1), I_3x3, Z_3x1)); @@ -51,7 +51,7 @@ inline HybridFactorGraph::shared_ptr makeSwitchingChain( } } - return boost::make_shared(std::move(hfg)); + return boost::make_shared(std::move(hfg)); } inline std::pair> makeBinaryOrdering( diff --git a/gtsam/hybrid/tests/testHybridFactorGraph.cpp b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp similarity index 92% rename from gtsam/hybrid/tests/testHybridFactorGraph.cpp rename to gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp index 860fdfdb8..552bb18f5 100644 --- a/gtsam/hybrid/tests/testHybridFactorGraph.cpp +++ b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /* - * @file testHybridFactorGraph.cpp + * @file testHybridGaussianFactorGraph.cpp * @date Mar 11, 2022 * @author Fan Jiang */ @@ -20,16 +20,16 @@ #include #include #include -#include +#include #include #include #include #include #include #include -#include #include -#include +#include +#include #include #include #include @@ -57,31 +57,17 @@ using gtsam::symbol_shorthand::D; using gtsam::symbol_shorthand::X; using gtsam::symbol_shorthand::Y; -#ifdef HYBRID_DEBUG -#define BOOST_STACKTRACE_GNU_SOURCE_NOT_REQUIRED - -#include // ::signal, ::raise - -#include - -void my_signal_handler(int signum) { - ::signal(signum, SIG_DFL); - std::cout << boost::stacktrace::stacktrace(); - ::raise(SIGABRT); -} -#endif - /* ************************************************************************* */ -TEST(HybridFactorGraph, creation) { +TEST(HybridGaussianFactorGraph, creation) { HybridConditional test; - HybridFactorGraph hfg; + HybridGaussianFactorGraph hfg; hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); - GaussianMixtureConditional clgc( + GaussianMixture clgc( {X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}), - GaussianMixtureConditional::Conditionals( + GaussianMixture::Conditionals( C(0), boost::make_shared(X(0), Z_3x1, I_3x3, X(1), I_3x3), @@ -91,8 +77,8 @@ TEST(HybridFactorGraph, creation) { } /* ************************************************************************* */ -TEST(HybridFactorGraph, eliminate) { - HybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminate) { + HybridGaussianFactorGraph hfg; hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); @@ -102,8 +88,8 @@ TEST(HybridFactorGraph, eliminate) { } /* ************************************************************************* */ -TEST(HybridFactorGraph, eliminateMultifrontal) { - HybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateMultifrontal) { + HybridGaussianFactorGraph hfg; DiscreteKey c(C(1), 2); @@ -119,8 +105,8 @@ TEST(HybridFactorGraph, eliminateMultifrontal) { } /* ************************************************************************* */ -TEST(HybridFactorGraph, eliminateFullSequentialEqualChance) { - HybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) { + HybridGaussianFactorGraph hfg; DiscreteKey c1(C(1), 2); @@ -143,8 +129,8 @@ TEST(HybridFactorGraph, eliminateFullSequentialEqualChance) { } /* ************************************************************************* */ -TEST(HybridFactorGraph, eliminateFullSequentialSimple) { - HybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) { + HybridGaussianFactorGraph hfg; DiscreteKey c1(C(1), 2); @@ -171,8 +157,8 @@ TEST(HybridFactorGraph, eliminateFullSequentialSimple) { } /* ************************************************************************* */ -TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) { - HybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) { + HybridGaussianFactorGraph hfg; DiscreteKey c1(C(1), 2); @@ -204,8 +190,8 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) { } /* ************************************************************************* */ -TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) { - HybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) { + HybridGaussianFactorGraph hfg; DiscreteKey c(C(1), 2); @@ -240,8 +226,8 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) { * This test is about how to assemble the Bayes Tree roots after we do partial * elimination */ -TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) { - HybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) { + HybridGaussianFactorGraph hfg; hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1)); @@ -290,7 +276,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) { GTSAM_PRINT(ordering_full); HybridBayesTree::shared_ptr hbt; - HybridFactorGraph::shared_ptr remaining; + HybridGaussianFactorGraph::shared_ptr remaining; std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal(ordering_full); GTSAM_PRINT(*hbt); @@ -309,7 +295,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) { /* ************************************************************************* */ // TODO(fan): make a graph like Varun's paper one -TEST(HybridFactorGraph, Switching) { +TEST(HybridGaussianFactorGraph, Switching) { auto N = 12; auto hfg = makeSwitchingChain(N); @@ -381,7 +367,7 @@ TEST(HybridFactorGraph, Switching) { GTSAM_PRINT(ordering_full); HybridBayesTree::shared_ptr hbt; - HybridFactorGraph::shared_ptr remaining; + HybridGaussianFactorGraph::shared_ptr remaining; std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full); // GTSAM_PRINT(*hbt); @@ -417,7 +403,7 @@ TEST(HybridFactorGraph, Switching) { /* ************************************************************************* */ // TODO(fan): make a graph like Varun's paper one -TEST(HybridFactorGraph, SwitchingISAM) { +TEST(HybridGaussianFactorGraph, SwitchingISAM) { auto N = 11; auto hfg = makeSwitchingChain(N); @@ -473,7 +459,7 @@ TEST(HybridFactorGraph, SwitchingISAM) { GTSAM_PRINT(ordering_full); HybridBayesTree::shared_ptr hbt; - HybridFactorGraph::shared_ptr remaining; + HybridGaussianFactorGraph::shared_ptr remaining; std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full); // GTSAM_PRINT(*hbt); @@ -499,10 +485,10 @@ TEST(HybridFactorGraph, SwitchingISAM) { } auto new_fg = makeSwitchingChain(12); - auto isam = HybridISAM(*hbt); + auto isam = HybridGaussianISAM(*hbt); { - HybridFactorGraph factorGraph; + HybridGaussianFactorGraph factorGraph; factorGraph.push_back(new_fg->at(new_fg->size() - 2)); factorGraph.push_back(new_fg->at(new_fg->size() - 1)); isam.update(factorGraph); @@ -512,7 +498,7 @@ TEST(HybridFactorGraph, SwitchingISAM) { } /* ************************************************************************* */ -TEST(HybridFactorGraph, SwitchingTwoVar) { +TEST(HybridGaussianFactorGraph, SwitchingTwoVar) { const int N = 7; auto hfg = makeSwitchingChain(N, X); hfg->push_back(*makeSwitchingChain(N, Y, D)); @@ -582,7 +568,7 @@ TEST(HybridFactorGraph, SwitchingTwoVar) { } { HybridBayesNet::shared_ptr hbn; - HybridFactorGraph::shared_ptr remaining; + HybridGaussianFactorGraph::shared_ptr remaining; std::tie(hbn, remaining) = hfg->eliminatePartialSequential(ordering_partial); diff --git a/gtsam/inference/inference.i b/gtsam/inference/inference.i index bbf1b2daa..fbdd70fdf 100644 --- a/gtsam/inference/inference.i +++ b/gtsam/inference/inference.i @@ -9,7 +9,7 @@ namespace gtsam { #include #include #include -#include +#include #include @@ -107,36 +107,36 @@ class Ordering { template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> static gtsam::Ordering ColamdConstrainedLast( const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainLast, bool forceOrder = false); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> static gtsam::Ordering ColamdConstrainedFirst( const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainFirst, bool forceOrder = false); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> static gtsam::Ordering Natural(const FACTOR_GRAPH& graph); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> static gtsam::Ordering Metis(const FACTOR_GRAPH& graph); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, - gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}> + gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> static gtsam::Ordering Create(gtsam::Ordering::OrderingType orderingType, const FACTOR_GRAPH& graph); diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py index 144183816..07a8178e6 100644 --- a/python/gtsam/tests/test_HybridFactorGraph.py +++ b/python/gtsam/tests/test_HybridFactorGraph.py @@ -20,8 +20,8 @@ from gtsam.symbol_shorthand import C, X from gtsam.utils.test_case import GtsamTestCase -class TestHybridFactorGraph(GtsamTestCase): - """Unit tests for HybridFactorGraph.""" +class TestHybridGaussianFactorGraph(GtsamTestCase): + """Unit tests for HybridGaussianFactorGraph.""" def test_create(self): """Test contruction of hybrid factor graph.""" @@ -36,13 +36,14 @@ class TestHybridFactorGraph(GtsamTestCase): gmf = gtsam.GaussianMixtureFactor.FromFactors([X(0)], dk, [jf1, jf2]) - hfg = gtsam.HybridFactorGraph() + hfg = gtsam.HybridGaussianFactorGraph() hfg.add(jf1) hfg.add(jf2) hfg.push_back(gmf) hbn = hfg.eliminateSequential( - gtsam.Ordering.ColamdConstrainedLastHybridFactorGraph(hfg, [C(0)])) + gtsam.Ordering.ColamdConstrainedLastHybridGaussianFactorGraph( + hfg, [C(0)])) # print("hbn = ", hbn) self.assertEqual(hbn.size(), 2)