From 852a9b9ef65ff4f621f843d635484ca8b8375ccb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 28 May 2022 14:24:03 -0400 Subject: [PATCH 1/5] rename HybridFactorGraph to GaussianHybridFactorGraph --- ...raph.cpp => GaussianHybridFactorGraph.cpp} | 24 +++++---- ...torGraph.h => GaussianHybridFactorGraph.h} | 35 +++++++------ gtsam/hybrid/HybridBayesTree.cpp | 3 +- gtsam/hybrid/HybridBayesTree.h | 8 +-- gtsam/hybrid/HybridConditional.h | 7 +-- gtsam/hybrid/HybridEliminationTree.cpp | 8 +-- gtsam/hybrid/HybridEliminationTree.h | 10 ++-- gtsam/hybrid/HybridISAM.cpp | 6 +-- gtsam/hybrid/HybridISAM.h | 7 +-- gtsam/hybrid/HybridJunctionTree.cpp | 16 +++--- gtsam/hybrid/HybridJunctionTree.h | 6 +-- gtsam/hybrid/hybrid.i | 16 +++--- gtsam/hybrid/tests/Switching.h | 8 +-- ....cpp => testGaussianHybridFactorGraph.cpp} | 52 +++++++++---------- gtsam/inference/inference.i | 14 ++--- python/gtsam/tests/test_HybridFactorGraph.py | 9 ++-- 16 files changed, 122 insertions(+), 107 deletions(-) rename gtsam/hybrid/{HybridFactorGraph.cpp => GaussianHybridFactorGraph.cpp} (94%) rename gtsam/hybrid/{HybridFactorGraph.h => GaussianHybridFactorGraph.h} (74%) rename gtsam/hybrid/tests/{testHybridFactorGraph.cpp => testGaussianHybridFactorGraph.cpp} (93%) diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/GaussianHybridFactorGraph.cpp similarity index 94% rename from gtsam/hybrid/HybridFactorGraph.cpp rename to gtsam/hybrid/GaussianHybridFactorGraph.cpp index dbb4a62b3..3354d5b4d 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/GaussianHybridFactorGraph.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridFactorGraph.cpp + * @file GaussianHybridFactorGraph.cpp * @brief Hybrid factor graph that uses type erasure * @author Fan Jiang * @author Varun Agrawal @@ -23,13 +23,13 @@ #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 GaussianHybridFactorGraph &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 GaussianHybridFactorGraph &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 GaussianHybridFactorGraph &factors, + const Ordering &frontalKeys, const KeySet &continuousSeparator, const std::set &discreteSeparatorSet) { // NOTE: since we use the special JunctionTree, @@ -235,7 +236,8 @@ hybridElimination(const HybridFactorGraph &factors, const Ordering &frontalKeys, } /* ************************************************************************ */ std::pair // -EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { +EliminateHybrid(const GaussianHybridFactorGraph &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 @@ -343,22 +345,22 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { } /* ************************************************************************ */ -void HybridFactorGraph::add(JacobianFactor &&factor) { +void GaussianHybridFactorGraph::add(JacobianFactor &&factor) { FactorGraph::add(boost::make_shared(std::move(factor))); } /* ************************************************************************ */ -void HybridFactorGraph::add(JacobianFactor::shared_ptr factor) { +void GaussianHybridFactorGraph::add(JacobianFactor::shared_ptr factor) { FactorGraph::add(boost::make_shared(factor)); } /* ************************************************************************ */ -void HybridFactorGraph::add(DecisionTreeFactor &&factor) { +void GaussianHybridFactorGraph::add(DecisionTreeFactor &&factor) { FactorGraph::add(boost::make_shared(std::move(factor))); } /* ************************************************************************ */ -void HybridFactorGraph::add(DecisionTreeFactor::shared_ptr factor) { +void GaussianHybridFactorGraph::add(DecisionTreeFactor::shared_ptr factor) { FactorGraph::add(boost::make_shared(factor)); } diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/GaussianHybridFactorGraph.h similarity index 74% rename from gtsam/hybrid/HybridFactorGraph.h rename to gtsam/hybrid/GaussianHybridFactorGraph.h index 10670b6b7..c8e0718fc 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/GaussianHybridFactorGraph.h @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridFactorGraph.h - * @brief Hybrid factor graph that uses type erasure + * @file GaussianHybridFactorGraph.h + * @brief Linearized Hybrid factor graph that uses type erasure * @author Fan Jiang * @date Mar 11, 2022 */ @@ -26,7 +26,7 @@ namespace gtsam { // Forward declarations -class HybridFactorGraph; +class GaussianHybridFactorGraph; class HybridConditional; class HybridBayesNet; class HybridEliminationTree; @@ -36,17 +36,18 @@ class DecisionTreeFactor; class JacobianFactor; -/** Main elimination function for HybridFactorGraph */ +/** Main elimination function for GaussianHybridFactorGraph */ GTSAM_EXPORT std::pair, HybridFactor::shared_ptr> -EliminateHybrid(const HybridFactorGraph& factors, const Ordering& keys); +EliminateHybrid(const GaussianHybridFactorGraph& 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 GaussianHybridFactorGraph + FactorGraphType; ///< Type of the factor graph (e.g. + ///< GaussianHybridFactorGraph) typedef HybridConditional ConditionalType; ///< Type of conditionals from elimination typedef HybridBayesNet @@ -64,16 +65,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 GaussianHybridFactorGraph + : public FactorGraph, + public EliminateableFactorGraph { public: using Base = FactorGraph; - using This = HybridFactorGraph; ///< this class + using This = GaussianHybridFactorGraph; ///< this class using BaseEliminateable = EliminateableFactorGraph; ///< for elimination using shared_ptr = boost::shared_ptr; ///< shared_ptr to This @@ -84,7 +86,7 @@ class HybridFactorGraph : public FactorGraph, /// @name Constructors /// @{ - HybridFactorGraph() = default; + GaussianHybridFactorGraph() = default; /** * Implicit copy/downcast constructor to override explicit template container @@ -92,7 +94,8 @@ class HybridFactorGraph : public FactorGraph, * `cachedSeparatorMarginal_.reset(*separatorMarginal)` * */ template - HybridFactorGraph(const FactorGraph& graph) : Base(graph) {} + GaussianHybridFactorGraph(const FactorGraph& graph) + : Base(graph) {} /// @} diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index c8d70e67e..400cac5e7 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -25,7 +25,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..610181458 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -18,8 +18,8 @@ #pragma once +#include #include -#include #include #include #include @@ -37,10 +37,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.h b/gtsam/hybrid/HybridConditional.h index b942773cb..9592b0c69 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 @@ -34,7 +34,7 @@ namespace gtsam { -class HybridFactorGraph; +class GaussianHybridFactorGraph; /** * Hybrid Conditional Density @@ -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..148685327 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 GaussianHybridFactorGraph& factorGraph, + const VariableIndex& structure, const Ordering& order) : Base(factorGraph, structure, order) {} /* ************************************************************************* */ HybridEliminationTree::HybridEliminationTree( - const HybridFactorGraph& factorGraph, const Ordering& order) + const GaussianHybridFactorGraph& factorGraph, const Ordering& order) : Base(factorGraph, order) {} /* ************************************************************************* */ diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h index 27766724a..04bd9cd35 100644 --- a/gtsam/hybrid/HybridEliminationTree.h +++ b/gtsam/hybrid/HybridEliminationTree.h @@ -17,8 +17,8 @@ #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 GaussianHybridFactorGraph& 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 GaussianHybridFactorGraph& factorGraph, const Ordering& order); /// @} diff --git a/gtsam/hybrid/HybridISAM.cpp b/gtsam/hybrid/HybridISAM.cpp index 057f784ad..476149126 100644 --- a/gtsam/hybrid/HybridISAM.cpp +++ b/gtsam/hybrid/HybridISAM.cpp @@ -17,8 +17,8 @@ * @author Richard Roberts */ +#include #include -#include #include #include #include @@ -37,7 +37,7 @@ HybridISAM::HybridISAM() {} HybridISAM::HybridISAM(const HybridBayesTree& bayesTree) : Base(bayesTree) {} /* ************************************************************************* */ -void HybridISAM::updateInternal(const HybridFactorGraph& newFactors, +void HybridISAM::updateInternal(const GaussianHybridFactorGraph& newFactors, HybridBayesTree::Cliques* orphans, const HybridBayesTree::Eliminate& function) { // Remove the contaminated part of the Bayes tree @@ -90,7 +90,7 @@ void HybridISAM::updateInternal(const HybridFactorGraph& newFactors, } /* ************************************************************************* */ -void HybridISAM::update(const HybridFactorGraph& newFactors, +void HybridISAM::update(const GaussianHybridFactorGraph& newFactors, const HybridBayesTree::Eliminate& function) { Cliques orphans; this->updateInternal(newFactors, &orphans, function); diff --git a/gtsam/hybrid/HybridISAM.h b/gtsam/hybrid/HybridISAM.h index 0738f46c8..fae7efafd 100644 --- a/gtsam/hybrid/HybridISAM.h +++ b/gtsam/hybrid/HybridISAM.h @@ -20,8 +20,8 @@ #pragma once #include +#include #include -#include #include namespace gtsam { @@ -46,7 +46,8 @@ class GTSAM_EXPORT HybridISAM : public ISAM { private: /// Internal method that performs the ISAM update. void updateInternal( - const HybridFactorGraph& newFactors, HybridBayesTree::Cliques* orphans, + const GaussianHybridFactorGraph& newFactors, + HybridBayesTree::Cliques* orphans, const HybridBayesTree::Eliminate& function = HybridBayesTree::EliminationTraitsType::DefaultEliminate); @@ -57,7 +58,7 @@ 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 GaussianHybridFactorGraph& newFactors, const HybridBayesTree::Eliminate& function = HybridBayesTree::EliminationTraitsType::DefaultEliminate); }; diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index 981cd6f32..8fa3aa033 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -15,8 +15,8 @@ * @author Fan Jiang */ +#include #include -#include #include #include #include @@ -26,13 +26,17 @@ 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; diff --git a/gtsam/hybrid/HybridJunctionTree.h b/gtsam/hybrid/HybridJunctionTree.h index 824fa4f85..ce9b818e6 100644 --- a/gtsam/hybrid/HybridJunctionTree.h +++ b/gtsam/hybrid/HybridJunctionTree.h @@ -17,8 +17,8 @@ #pragma once +#include #include -#include #include namespace gtsam { @@ -49,9 +49,9 @@ class HybridEliminationTree; * \nosubgrouping */ class GTSAM_EXPORT HybridJunctionTree - : public JunctionTree { + : public JunctionTree { public: - typedef JunctionTree + typedef JunctionTree Base; ///< Base class typedef HybridJunctionTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index d3ff98719..612f3abc5 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -98,15 +98,15 @@ class HybridBayesNet { const gtsam::DotWriter& writer = gtsam::DotWriter()) const; }; -#include -class HybridFactorGraph { - HybridFactorGraph(); - HybridFactorGraph(const gtsam::HybridBayesNet& bayesNet); +#include +class GaussianHybridFactorGraph { + GaussianHybridFactorGraph(); + GaussianHybridFactorGraph(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::GaussianHybridFactorGraph& 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::GaussianHybridFactorGraph& 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..77d8182c8 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -18,8 +18,8 @@ #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 GaussianHybridFactorGraph::shared_ptr makeSwitchingChain( size_t n, std::function keyFunc = X, std::function dKeyFunc = C) { - HybridFactorGraph hfg; + GaussianHybridFactorGraph 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 93% rename from gtsam/hybrid/tests/testHybridFactorGraph.cpp rename to gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp index 860fdfdb8..853353278 100644 --- a/gtsam/hybrid/tests/testHybridFactorGraph.cpp +++ b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /* - * @file testHybridFactorGraph.cpp + * @file testGaussianHybridFactorGraph.cpp * @date Mar 11, 2022 * @author Fan Jiang */ @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -27,7 +28,6 @@ #include #include #include -#include #include #include #include @@ -72,10 +72,10 @@ void my_signal_handler(int signum) { #endif /* ************************************************************************* */ -TEST(HybridFactorGraph, creation) { +TEST(GaussianHybridFactorGraph, creation) { HybridConditional test; - HybridFactorGraph hfg; + GaussianHybridFactorGraph hfg; hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); @@ -91,8 +91,8 @@ TEST(HybridFactorGraph, creation) { } /* ************************************************************************* */ -TEST(HybridFactorGraph, eliminate) { - HybridFactorGraph hfg; +TEST(GaussianHybridFactorGraph, eliminate) { + GaussianHybridFactorGraph hfg; hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); @@ -102,8 +102,8 @@ TEST(HybridFactorGraph, eliminate) { } /* ************************************************************************* */ -TEST(HybridFactorGraph, eliminateMultifrontal) { - HybridFactorGraph hfg; +TEST(GaussianHybridFactorGraph, eliminateMultifrontal) { + GaussianHybridFactorGraph hfg; DiscreteKey c(C(1), 2); @@ -119,8 +119,8 @@ TEST(HybridFactorGraph, eliminateMultifrontal) { } /* ************************************************************************* */ -TEST(HybridFactorGraph, eliminateFullSequentialEqualChance) { - HybridFactorGraph hfg; +TEST(GaussianHybridFactorGraph, eliminateFullSequentialEqualChance) { + GaussianHybridFactorGraph hfg; DiscreteKey c1(C(1), 2); @@ -143,8 +143,8 @@ TEST(HybridFactorGraph, eliminateFullSequentialEqualChance) { } /* ************************************************************************* */ -TEST(HybridFactorGraph, eliminateFullSequentialSimple) { - HybridFactorGraph hfg; +TEST(GaussianHybridFactorGraph, eliminateFullSequentialSimple) { + GaussianHybridFactorGraph hfg; DiscreteKey c1(C(1), 2); @@ -171,8 +171,8 @@ TEST(HybridFactorGraph, eliminateFullSequentialSimple) { } /* ************************************************************************* */ -TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) { - HybridFactorGraph hfg; +TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalSimple) { + GaussianHybridFactorGraph hfg; DiscreteKey c1(C(1), 2); @@ -204,8 +204,8 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) { } /* ************************************************************************* */ -TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) { - HybridFactorGraph hfg; +TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalCLG) { + GaussianHybridFactorGraph hfg; DiscreteKey c(C(1), 2); @@ -240,8 +240,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(GaussianHybridFactorGraph, eliminateFullMultifrontalTwoClique) { + GaussianHybridFactorGraph 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 +290,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) { GTSAM_PRINT(ordering_full); HybridBayesTree::shared_ptr hbt; - HybridFactorGraph::shared_ptr remaining; + GaussianHybridFactorGraph::shared_ptr remaining; std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal(ordering_full); GTSAM_PRINT(*hbt); @@ -309,7 +309,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) { /* ************************************************************************* */ // TODO(fan): make a graph like Varun's paper one -TEST(HybridFactorGraph, Switching) { +TEST(GaussianHybridFactorGraph, Switching) { auto N = 12; auto hfg = makeSwitchingChain(N); @@ -381,7 +381,7 @@ TEST(HybridFactorGraph, Switching) { GTSAM_PRINT(ordering_full); HybridBayesTree::shared_ptr hbt; - HybridFactorGraph::shared_ptr remaining; + GaussianHybridFactorGraph::shared_ptr remaining; std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full); // GTSAM_PRINT(*hbt); @@ -417,7 +417,7 @@ TEST(HybridFactorGraph, Switching) { /* ************************************************************************* */ // TODO(fan): make a graph like Varun's paper one -TEST(HybridFactorGraph, SwitchingISAM) { +TEST(GaussianHybridFactorGraph, SwitchingISAM) { auto N = 11; auto hfg = makeSwitchingChain(N); @@ -473,7 +473,7 @@ TEST(HybridFactorGraph, SwitchingISAM) { GTSAM_PRINT(ordering_full); HybridBayesTree::shared_ptr hbt; - HybridFactorGraph::shared_ptr remaining; + GaussianHybridFactorGraph::shared_ptr remaining; std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full); // GTSAM_PRINT(*hbt); @@ -502,7 +502,7 @@ TEST(HybridFactorGraph, SwitchingISAM) { auto isam = HybridISAM(*hbt); { - HybridFactorGraph factorGraph; + GaussianHybridFactorGraph 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 +512,7 @@ TEST(HybridFactorGraph, SwitchingISAM) { } /* ************************************************************************* */ -TEST(HybridFactorGraph, SwitchingTwoVar) { +TEST(GaussianHybridFactorGraph, SwitchingTwoVar) { const int N = 7; auto hfg = makeSwitchingChain(N, X); hfg->push_back(*makeSwitchingChain(N, Y, D)); @@ -582,7 +582,7 @@ TEST(HybridFactorGraph, SwitchingTwoVar) { } { HybridBayesNet::shared_ptr hbn; - HybridFactorGraph::shared_ptr remaining; + GaussianHybridFactorGraph::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..e8d918a1d 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::GaussianHybridFactorGraph}> 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::GaussianHybridFactorGraph}> 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::GaussianHybridFactorGraph}> 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::GaussianHybridFactorGraph}> 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::GaussianHybridFactorGraph}> 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::GaussianHybridFactorGraph}> 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..895c9e14e 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 TestGaussianHybridFactorGraph(GtsamTestCase): + """Unit tests for GaussianHybridFactorGraph.""" 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.GaussianHybridFactorGraph() hfg.add(jf1) hfg.add(jf2) hfg.push_back(gmf) hbn = hfg.eliminateSequential( - gtsam.Ordering.ColamdConstrainedLastHybridFactorGraph(hfg, [C(0)])) + gtsam.Ordering.ColamdConstrainedLastGaussianHybridFactorGraph( + hfg, [C(0)])) # print("hbn = ", hbn) self.assertEqual(hbn.size(), 2) From 7c7b5dd03096c1e1516a964563b0b840a2b77a45 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Jun 2022 23:52:21 -0400 Subject: [PATCH 2/5] Rename files so that everything is HybridX --- gtsam/hybrid/HybridBayesTree.cpp | 5 +- gtsam/hybrid/HybridBayesTree.h | 9 +-- gtsam/hybrid/HybridConditional.h | 4 +- gtsam/hybrid/HybridEliminationTree.cpp | 6 +- gtsam/hybrid/HybridEliminationTree.h | 10 +-- ...raph.cpp => HybridGaussianFactorGraph.cpp} | 24 +++---- ...torGraph.h => HybridGaussianFactorGraph.h} | 33 ++++----- ...{HybridISAM.cpp => HybridGaussianISAM.cpp} | 22 +++--- .../{HybridISAM.h => HybridGaussianISAM.h} | 18 ++--- ...ree.cpp => HybridGaussianJunctionTree.cpp} | 70 +++++++++---------- ...ionTree.h => HybridGaussianJunctionTree.h} | 14 ++-- gtsam/hybrid/hybrid.i | 16 ++--- gtsam/hybrid/tests/Switching.h | 8 +-- .../tests/testGaussianHybridFactorGraph.cpp | 70 ++++++++----------- gtsam/inference/inference.i | 14 ++-- python/gtsam/tests/test_HybridFactorGraph.py | 8 +-- 16 files changed, 161 insertions(+), 170 deletions(-) rename gtsam/hybrid/{GaussianHybridFactorGraph.cpp => HybridGaussianFactorGraph.cpp} (94%) rename gtsam/hybrid/{GaussianHybridFactorGraph.h => HybridGaussianFactorGraph.h} (77%) rename gtsam/hybrid/{HybridISAM.cpp => HybridGaussianISAM.cpp} (84%) rename gtsam/hybrid/{HybridISAM.h => HybridGaussianISAM.h} (76%) rename gtsam/hybrid/{HybridJunctionTree.cpp => HybridGaussianJunctionTree.cpp} (73%) rename gtsam/hybrid/{HybridJunctionTree.h => HybridGaussianJunctionTree.h} (84%) diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index 400cac5e7..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 */ @@ -26,7 +27,7 @@ namespace gtsam { // Instantiate base class template class BayesTreeCliqueBase; + HybridGaussianFactorGraph>; template class BayesTree; /* ************************************************************************* */ diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index 610181458..28d9ef809 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -11,15 +11,16 @@ /** * @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 */ #pragma once -#include #include +#include #include #include #include @@ -38,10 +39,10 @@ class VectorValues; */ class GTSAM_EXPORT HybridBayesTreeClique : public BayesTreeCliqueBase { + HybridGaussianFactorGraph> { public: typedef HybridBayesTreeClique This; - typedef BayesTreeCliqueBase + typedef BayesTreeCliqueBase Base; typedef boost::shared_ptr shared_ptr; typedef boost::weak_ptr weak_ptr; diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index 9592b0c69..887b49f12 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 @@ -34,7 +34,7 @@ namespace gtsam { -class GaussianHybridFactorGraph; +class HybridGaussianFactorGraph; /** * Hybrid Conditional Density diff --git a/gtsam/hybrid/HybridEliminationTree.cpp b/gtsam/hybrid/HybridEliminationTree.cpp index 148685327..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 GaussianHybridFactorGraph& factorGraph, + const HybridGaussianFactorGraph& factorGraph, const VariableIndex& structure, const Ordering& order) : Base(factorGraph, structure, order) {} /* ************************************************************************* */ HybridEliminationTree::HybridEliminationTree( - const GaussianHybridFactorGraph& 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 04bd9cd35..77a84fea8 100644 --- a/gtsam/hybrid/HybridEliminationTree.h +++ b/gtsam/hybrid/HybridEliminationTree.h @@ -17,8 +17,8 @@ #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 GaussianHybridFactorGraph& 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 GaussianHybridFactorGraph& factorGraph, + HybridEliminationTree(const HybridGaussianFactorGraph& factorGraph, const Ordering& order); /// @} diff --git a/gtsam/hybrid/GaussianHybridFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp similarity index 94% rename from gtsam/hybrid/GaussianHybridFactorGraph.cpp rename to gtsam/hybrid/HybridGaussianFactorGraph.cpp index 3354d5b4d..0ac2c2656 100644 --- a/gtsam/hybrid/GaussianHybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file GaussianHybridFactorGraph.cpp + * @file HybridGaussianFactorGraph.cpp * @brief Hybrid factor graph that uses type erasure * @author Fan Jiang * @author Varun Agrawal @@ -23,7 +23,6 @@ #include #include #include -#include #include #include #include @@ -31,7 +30,8 @@ #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 GaussianHybridFactorGraph &factors, +continuousElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) { GaussianFactorGraph gfg; for (auto &fp : factors) { @@ -103,7 +103,7 @@ continuousElimination(const GaussianHybridFactorGraph &factors, /* ************************************************************************ */ std::pair -discreteElimination(const GaussianHybridFactorGraph &factors, +discreteElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) { DiscreteFactorGraph dfg; for (auto &fp : factors) { @@ -129,7 +129,7 @@ discreteElimination(const GaussianHybridFactorGraph &factors, /* ************************************************************************ */ std::pair -hybridElimination(const GaussianHybridFactorGraph &factors, +hybridElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys, const KeySet &continuousSeparator, const std::set &discreteSeparatorSet) { @@ -236,7 +236,7 @@ hybridElimination(const GaussianHybridFactorGraph &factors, } /* ************************************************************************ */ std::pair // -EliminateHybrid(const GaussianHybridFactorGraph &factors, +EliminateHybrid(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) { // NOTE: Because we are in the Conditional Gaussian regime there are only // a few cases: @@ -345,22 +345,22 @@ EliminateHybrid(const GaussianHybridFactorGraph &factors, } /* ************************************************************************ */ -void GaussianHybridFactorGraph::add(JacobianFactor &&factor) { +void HybridGaussianFactorGraph::add(JacobianFactor &&factor) { FactorGraph::add(boost::make_shared(std::move(factor))); } /* ************************************************************************ */ -void GaussianHybridFactorGraph::add(JacobianFactor::shared_ptr factor) { +void HybridGaussianFactorGraph::add(JacobianFactor::shared_ptr factor) { FactorGraph::add(boost::make_shared(factor)); } /* ************************************************************************ */ -void GaussianHybridFactorGraph::add(DecisionTreeFactor &&factor) { +void HybridGaussianFactorGraph::add(DecisionTreeFactor &&factor) { FactorGraph::add(boost::make_shared(std::move(factor))); } /* ************************************************************************ */ -void GaussianHybridFactorGraph::add(DecisionTreeFactor::shared_ptr factor) { +void HybridGaussianFactorGraph::add(DecisionTreeFactor::shared_ptr factor) { FactorGraph::add(boost::make_shared(factor)); } diff --git a/gtsam/hybrid/GaussianHybridFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h similarity index 77% rename from gtsam/hybrid/GaussianHybridFactorGraph.h rename to gtsam/hybrid/HybridGaussianFactorGraph.h index c8e0718fc..6944af510 100644 --- a/gtsam/hybrid/GaussianHybridFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file GaussianHybridFactorGraph.h + * @file HybridGaussianFactorGraph.h * @brief Linearized Hybrid factor graph that uses type erasure * @author Fan Jiang * @date Mar 11, 2022 @@ -26,36 +26,37 @@ namespace gtsam { // Forward declarations -class GaussianHybridFactorGraph; +class HybridGaussianFactorGraph; class HybridConditional; class HybridBayesNet; class HybridEliminationTree; class HybridBayesTree; -class HybridJunctionTree; +class HybridGaussianJunctionTree; class DecisionTreeFactor; class JacobianFactor; -/** Main elimination function for GaussianHybridFactorGraph */ +/** Main elimination function for HybridGaussianFactorGraph */ GTSAM_EXPORT std::pair, HybridFactor::shared_ptr> -EliminateHybrid(const GaussianHybridFactorGraph& 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 GaussianHybridFactorGraph + typedef HybridGaussianFactorGraph FactorGraphType; ///< Type of the factor graph (e.g. - ///< GaussianHybridFactorGraph) + ///< 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 > @@ -70,12 +71,12 @@ struct EliminationTraits { * This is the linearized version of a hybrid factor graph. * Everything inside needs to be hybrid factor or hybrid conditional. */ -class GaussianHybridFactorGraph +class HybridGaussianFactorGraph : public FactorGraph, - public EliminateableFactorGraph { + public EliminateableFactorGraph { public: using Base = FactorGraph; - using This = GaussianHybridFactorGraph; ///< this class + using This = HybridGaussianFactorGraph; ///< this class using BaseEliminateable = EliminateableFactorGraph; ///< for elimination using shared_ptr = boost::shared_ptr; ///< shared_ptr to This @@ -86,7 +87,7 @@ class GaussianHybridFactorGraph /// @name Constructors /// @{ - GaussianHybridFactorGraph() = default; + HybridGaussianFactorGraph() = default; /** * Implicit copy/downcast constructor to override explicit template container @@ -94,7 +95,7 @@ class GaussianHybridFactorGraph * `cachedSeparatorMarginal_.reset(*separatorMarginal)` * */ template - GaussianHybridFactorGraph(const FactorGraph& 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 476149126..7783a88dd 100644 --- a/gtsam/hybrid/HybridISAM.cpp +++ b/gtsam/hybrid/HybridGaussianISAM.cpp @@ -10,16 +10,16 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridISAM.h + * @file HybridGaussianISAM.h * @date March 31, 2022 * @author Fan Jiang * @author Frank Dellaert * @author Richard Roberts */ -#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 GaussianHybridFactorGraph& 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 GaussianHybridFactorGraph& newFactors, } /* ************************************************************************* */ -void HybridISAM::update(const GaussianHybridFactorGraph& 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 76% rename from gtsam/hybrid/HybridISAM.h rename to gtsam/hybrid/HybridGaussianISAM.h index fae7efafd..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 @@ -20,33 +20,33 @@ #pragma once #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 GaussianHybridFactorGraph& newFactors, + const HybridGaussianFactorGraph& newFactors, HybridBayesTree::Cliques* orphans, const HybridBayesTree::Eliminate& function = HybridBayesTree::EliminationTraitsType::DefaultEliminate); @@ -58,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 GaussianHybridFactorGraph& 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 73% rename from gtsam/hybrid/HybridJunctionTree.cpp rename to gtsam/hybrid/HybridGaussianJunctionTree.cpp index 8fa3aa033..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 @@ -27,19 +27,19 @@ namespace gtsam { // Instantiate base classes template class EliminatableClusterTree; -template class JunctionTree; + HybridGaussianFactorGraph>; +template class JunctionTree; struct HybridConstructorTraversalData { typedef - typename JunctionTree::Node + typename JunctionTree::Node Node; typedef typename JunctionTree::sharedNode sharedNode; + HybridGaussianFactorGraph>::sharedNode sharedNode; HybridConstructorTraversalData* const parentData; - sharedNode myJTNode; + sharedNode junctionTreeNode; FastVector childSymbolicConditionals; FastVector childSymbolicFactors; KeySet discreteKeys; @@ -57,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 @@ -87,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; } } @@ -142,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, @@ -156,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, @@ -164,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 ce9b818e6..314e7daa6 100644 --- a/gtsam/hybrid/HybridJunctionTree.h +++ b/gtsam/hybrid/HybridGaussianJunctionTree.h @@ -10,15 +10,15 @@ * -------------------------------------------------------------------------- */ /** - * @file HybridJunctionTree.h + * @file HybridGaussianJunctionTree.h * @date Mar 11, 2022 * @author Fan Jiang */ #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 612f3abc5..94eaa88b2 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -98,15 +98,15 @@ class HybridBayesNet { const gtsam::DotWriter& writer = gtsam::DotWriter()) const; }; -#include -class GaussianHybridFactorGraph { - GaussianHybridFactorGraph(); - GaussianHybridFactorGraph(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::GaussianHybridFactorGraph& 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 GaussianHybridFactorGraph { const gtsam::HybridFactor* at(size_t i) const; void print(string s = "") const; - bool equals(const gtsam::GaussianHybridFactorGraph& 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 GaussianHybridFactorGraph { 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 77d8182c8..c081b8e87 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -18,8 +18,8 @@ #include #include -#include #include +#include #include #include @@ -29,10 +29,10 @@ using gtsam::symbol_shorthand::C; using gtsam::symbol_shorthand::X; namespace gtsam { -inline GaussianHybridFactorGraph::shared_ptr makeSwitchingChain( +inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain( size_t n, std::function keyFunc = X, std::function dKeyFunc = C) { - GaussianHybridFactorGraph hfg; + HybridGaussianFactorGraph hfg; hfg.add(JacobianFactor(keyFunc(1), I_3x3, Z_3x1)); @@ -51,7 +51,7 @@ inline GaussianHybridFactorGraph::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/testGaussianHybridFactorGraph.cpp b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp index 853353278..8ff959d74 100644 --- a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp +++ b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /* - * @file testGaussianHybridFactorGraph.cpp + * @file testHybridGaussianFactorGraph.cpp * @date Mar 11, 2022 * @author Fan Jiang */ @@ -20,7 +20,6 @@ #include #include #include -#include #include #include #include @@ -29,7 +28,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -57,25 +57,11 @@ 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(GaussianHybridFactorGraph, creation) { +TEST(HybridGaussianFactorGraph, creation) { HybridConditional test; - GaussianHybridFactorGraph hfg; + HybridGaussianFactorGraph hfg; hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); @@ -91,8 +77,8 @@ TEST(GaussianHybridFactorGraph, creation) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, eliminate) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminate) { + HybridGaussianFactorGraph hfg; hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); @@ -102,8 +88,8 @@ TEST(GaussianHybridFactorGraph, eliminate) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, eliminateMultifrontal) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateMultifrontal) { + HybridGaussianFactorGraph hfg; DiscreteKey c(C(1), 2); @@ -119,8 +105,8 @@ TEST(GaussianHybridFactorGraph, eliminateMultifrontal) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, eliminateFullSequentialEqualChance) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) { + HybridGaussianFactorGraph hfg; DiscreteKey c1(C(1), 2); @@ -143,8 +129,8 @@ TEST(GaussianHybridFactorGraph, eliminateFullSequentialEqualChance) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, eliminateFullSequentialSimple) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) { + HybridGaussianFactorGraph hfg; DiscreteKey c1(C(1), 2); @@ -171,8 +157,8 @@ TEST(GaussianHybridFactorGraph, eliminateFullSequentialSimple) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalSimple) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) { + HybridGaussianFactorGraph hfg; DiscreteKey c1(C(1), 2); @@ -204,8 +190,8 @@ TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalSimple) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalCLG) { - GaussianHybridFactorGraph hfg; +TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) { + HybridGaussianFactorGraph hfg; DiscreteKey c(C(1), 2); @@ -240,8 +226,8 @@ TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalCLG) { * This test is about how to assemble the Bayes Tree roots after we do partial * elimination */ -TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalTwoClique) { - GaussianHybridFactorGraph 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(GaussianHybridFactorGraph, eliminateFullMultifrontalTwoClique) { GTSAM_PRINT(ordering_full); HybridBayesTree::shared_ptr hbt; - GaussianHybridFactorGraph::shared_ptr remaining; + HybridGaussianFactorGraph::shared_ptr remaining; std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal(ordering_full); GTSAM_PRINT(*hbt); @@ -309,7 +295,7 @@ TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalTwoClique) { /* ************************************************************************* */ // TODO(fan): make a graph like Varun's paper one -TEST(GaussianHybridFactorGraph, Switching) { +TEST(HybridGaussianFactorGraph, Switching) { auto N = 12; auto hfg = makeSwitchingChain(N); @@ -381,7 +367,7 @@ TEST(GaussianHybridFactorGraph, Switching) { GTSAM_PRINT(ordering_full); HybridBayesTree::shared_ptr hbt; - GaussianHybridFactorGraph::shared_ptr remaining; + HybridGaussianFactorGraph::shared_ptr remaining; std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full); // GTSAM_PRINT(*hbt); @@ -417,7 +403,7 @@ TEST(GaussianHybridFactorGraph, Switching) { /* ************************************************************************* */ // TODO(fan): make a graph like Varun's paper one -TEST(GaussianHybridFactorGraph, SwitchingISAM) { +TEST(HybridGaussianFactorGraph, SwitchingISAM) { auto N = 11; auto hfg = makeSwitchingChain(N); @@ -473,7 +459,7 @@ TEST(GaussianHybridFactorGraph, SwitchingISAM) { GTSAM_PRINT(ordering_full); HybridBayesTree::shared_ptr hbt; - GaussianHybridFactorGraph::shared_ptr remaining; + HybridGaussianFactorGraph::shared_ptr remaining; std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full); // GTSAM_PRINT(*hbt); @@ -499,10 +485,10 @@ TEST(GaussianHybridFactorGraph, SwitchingISAM) { } auto new_fg = makeSwitchingChain(12); - auto isam = HybridISAM(*hbt); + auto isam = HybridGaussianISAM(*hbt); { - GaussianHybridFactorGraph 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(GaussianHybridFactorGraph, SwitchingISAM) { } /* ************************************************************************* */ -TEST(GaussianHybridFactorGraph, 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(GaussianHybridFactorGraph, SwitchingTwoVar) { } { HybridBayesNet::shared_ptr hbn; - GaussianHybridFactorGraph::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 e8d918a1d..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::GaussianHybridFactorGraph}> + 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::GaussianHybridFactorGraph}> + 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::GaussianHybridFactorGraph}> + 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::GaussianHybridFactorGraph}> + 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::GaussianHybridFactorGraph}> + 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::GaussianHybridFactorGraph}> + 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 895c9e14e..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 TestGaussianHybridFactorGraph(GtsamTestCase): - """Unit tests for GaussianHybridFactorGraph.""" +class TestHybridGaussianFactorGraph(GtsamTestCase): + """Unit tests for HybridGaussianFactorGraph.""" def test_create(self): """Test contruction of hybrid factor graph.""" @@ -36,13 +36,13 @@ class TestGaussianHybridFactorGraph(GtsamTestCase): gmf = gtsam.GaussianMixtureFactor.FromFactors([X(0)], dk, [jf1, jf2]) - hfg = gtsam.GaussianHybridFactorGraph() + hfg = gtsam.HybridGaussianFactorGraph() hfg.add(jf1) hfg.add(jf2) hfg.push_back(gmf) hbn = hfg.eliminateSequential( - gtsam.Ordering.ColamdConstrainedLastGaussianHybridFactorGraph( + gtsam.Ordering.ColamdConstrainedLastHybridGaussianFactorGraph( hfg, [C(0)])) # print("hbn = ", hbn) From 31ab1a32f3b93c9c7d2e97d95e9bd1c47013ef73 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jun 2022 00:03:31 -0400 Subject: [PATCH 3/5] update description of GaussianMixtureConditional --- gtsam/hybrid/GaussianMixtureConditional.h | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/GaussianMixtureConditional.h b/gtsam/hybrid/GaussianMixtureConditional.h index d12fa09d7..a743c00f3 100644 --- a/gtsam/hybrid/GaussianMixtureConditional.h +++ b/gtsam/hybrid/GaussianMixtureConditional.h @@ -27,13 +27,19 @@ 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 GaussianMixtureConditional : public HybridFactor, public Conditional { public: From d2029f3d0339175fbf8b1e4fe0734e9b8e5a37bb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jun 2022 00:09:38 -0400 Subject: [PATCH 4/5] Add more information on conditionals requirement for GaussianMixture --- gtsam/hybrid/GaussianMixtureConditional.h | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/GaussianMixtureConditional.h b/gtsam/hybrid/GaussianMixtureConditional.h index a743c00f3..ab0e2ce43 100644 --- a/gtsam/hybrid/GaussianMixtureConditional.h +++ b/gtsam/hybrid/GaussianMixtureConditional.h @@ -68,13 +68,17 @@ class GTSAM_EXPORT GaussianMixtureConditional /// Defaut constructor, mainly for serialization. GaussianMixtureConditional() = default; + /** - * @brief Construct a new GaussianMixtureConditional object + * @brief Construct a new GaussianMixtureConditional 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, const KeyVector &continuousParents, From c8bf9d350ca9ace6de6218607dabc9357b7340a8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jun 2022 00:13:02 -0400 Subject: [PATCH 5/5] rename GaussianMixtureConditional to GaussianMixture --- ...ureConditional.cpp => GaussianMixture.cpp} | 28 +++++++++---------- ...MixtureConditional.h => GaussianMixture.h} | 20 ++++++------- gtsam/hybrid/HybridConditional.cpp | 2 +- gtsam/hybrid/HybridConditional.h | 14 +++++----- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 8 +++--- gtsam/hybrid/hybrid.i | 8 +++--- .../tests/testGaussianHybridFactorGraph.cpp | 6 ++-- 7 files changed, 43 insertions(+), 43 deletions(-) rename gtsam/hybrid/{GaussianMixtureConditional.cpp => GaussianMixture.cpp} (80%) rename gtsam/hybrid/{GaussianMixtureConditional.h => GaussianMixture.h} (87%) 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 726af6d5f..1663236f0 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,13 +77,13 @@ GaussianMixtureConditional::asGaussianFactorGraphTree() const { } /* *******************************************************************************/ -bool GaussianMixtureConditional::equals(const HybridFactor &lf, +bool GaussianMixture::equals(const HybridFactor &lf, double tol) const { return BaseFactor::equals(lf, 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 87% rename from gtsam/hybrid/GaussianMixtureConditional.h rename to gtsam/hybrid/GaussianMixture.h index ab0e2ce43..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 @@ -39,14 +39,14 @@ namespace gtsam { * coefficient. * */ -class GTSAM_EXPORT 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; @@ -67,10 +67,10 @@ class GTSAM_EXPORT 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. @@ -80,7 +80,7 @@ class GTSAM_EXPORT GaussianMixtureConditional * 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); @@ -107,7 +107,7 @@ class GTSAM_EXPORT 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/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index 7d1b72067..8f95caf34 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 887b49f12..3ba5da393 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -18,7 +18,7 @@ #pragma once #include -#include +#include #include #include #include @@ -42,7 +42,7 @@ class HybridGaussianFactorGraph; * 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_); } /** diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 0ac2c2656..f4f09701f 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include #include #include @@ -207,8 +207,8 @@ hybridElimination(const HybridGaussianFactorGraph &factors, 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 @@ -262,7 +262,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 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. diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 94eaa88b2..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; }; diff --git a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp index 8ff959d74..552bb18f5 100644 --- a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp +++ b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include #include @@ -65,9 +65,9 @@ TEST(HybridGaussianFactorGraph, creation) { 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),