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)