diff --git a/gtsam/hybrid/CGMixtureFactor.cpp b/gtsam/hybrid/CGMixtureFactor.cpp index 16ead783e..705160273 100644 --- a/gtsam/hybrid/CGMixtureFactor.cpp +++ b/gtsam/hybrid/CGMixtureFactor.cpp @@ -1,6 +1,22 @@ -// -// Created by Fan Jiang on 3/11/22. -// +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file CGMixtureFactor.cpp + * @brief A set of Gaussian factors indexed by a set of discrete keys. + * @author Fan Jiang + * @author Varun Agrawal + * @author Frank Dellaert + * @date Mar 12, 2022 + */ #include diff --git a/gtsam/hybrid/CGMixtureFactor.h b/gtsam/hybrid/CGMixtureFactor.h index 7ff53b7ed..0528d5401 100644 --- a/gtsam/hybrid/CGMixtureFactor.h +++ b/gtsam/hybrid/CGMixtureFactor.h @@ -18,15 +18,15 @@ * @date Mar 12, 2022 */ -#include -#include #include #include +#include +#include namespace gtsam { class CGMixtureFactor : public HybridFactor { -public: + public: using Base = HybridFactor; using This = CGMixtureFactor; using shared_ptr = boost::shared_ptr; @@ -38,13 +38,13 @@ public: CGMixtureFactor() = default; CGMixtureFactor(const KeyVector &continuousKeys, - const DiscreteKeys &discreteKeys, - const Factors &factors); + const DiscreteKeys &discreteKeys, const Factors &factors); bool equals(const HybridFactor &lf, double tol = 1e-9) const override; - void print(const std::string &s = "HybridFactor\n", - const KeyFormatter &formatter = DefaultKeyFormatter) const override; + void print( + const std::string &s = "HybridFactor\n", + const KeyFormatter &formatter = DefaultKeyFormatter) const override; }; -} +} // namespace gtsam diff --git a/gtsam/hybrid/CLGaussianConditional.cpp b/gtsam/hybrid/CLGaussianConditional.cpp index 37c82a0d5..30531c023 100644 --- a/gtsam/hybrid/CLGaussianConditional.cpp +++ b/gtsam/hybrid/CLGaussianConditional.cpp @@ -16,30 +16,28 @@ * @date Mar 12, 2022 */ -#include - #include - -#include #include +#include +#include namespace gtsam { -CLGaussianConditional::CLGaussianConditional(const KeyVector &continuousFrontals, - const KeyVector &continuousParents, - const DiscreteKeys &discreteParents, - const CLGaussianConditional::Conditionals &conditionals) - : BaseFactor( - CollectKeys(continuousFrontals, continuousParents), discreteParents), - BaseConditional(continuousFrontals.size()), conditionals_(conditionals) { - -} +CLGaussianConditional::CLGaussianConditional( + const KeyVector &continuousFrontals, const KeyVector &continuousParents, + const DiscreteKeys &discreteParents, + const CLGaussianConditional::Conditionals &conditionals) + : BaseFactor(CollectKeys(continuousFrontals, continuousParents), + discreteParents), + BaseConditional(continuousFrontals.size()), + conditionals_(conditionals) {} bool CLGaussianConditional::equals(const HybridFactor &lf, double tol) const { return false; } -void CLGaussianConditional::print(const std::string &s, const KeyFormatter &formatter) const { +void CLGaussianConditional::print(const std::string &s, + const KeyFormatter &formatter) const { std::cout << s << ": "; if (isContinuous_) std::cout << "Cont. "; if (isDiscrete_) std::cout << "Disc. "; @@ -51,14 +49,14 @@ void CLGaussianConditional::print(const std::string &s, const KeyFormatter &form } std::cout << "\n"; conditionals_.print( - "", - [&](Key k) { - return formatter(k); - }, [&](const GaussianConditional::shared_ptr &gf) -> std::string { + "", [&](Key k) { return formatter(k); }, + [&](const GaussianConditional::shared_ptr &gf) -> std::string { RedirectCout rd; - if (!gf->empty()) gf->print("", formatter); - else return {"nullptr"}; + if (!gf->empty()) + gf->print("", formatter); + else + return {"nullptr"}; return rd.str(); }); } -} \ No newline at end of file +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/hybrid/CLGaussianConditional.h b/gtsam/hybrid/CLGaussianConditional.h index 0319c60a7..0429f4835 100644 --- a/gtsam/hybrid/CLGaussianConditional.h +++ b/gtsam/hybrid/CLGaussianConditional.h @@ -16,15 +16,16 @@ * @date Mar 12, 2022 */ -#include -#include - -#include #include +#include +#include +#include namespace gtsam { -class CLGaussianConditional : public HybridFactor, public Conditional { -public: +class CLGaussianConditional + : public HybridFactor, + public Conditional { + public: using This = CLGaussianConditional; using shared_ptr = boost::shared_ptr; using BaseFactor = HybridFactor; @@ -34,8 +35,7 @@ public: Conditionals conditionals_; -public: - + public: CLGaussianConditional(const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, @@ -47,4 +47,4 @@ public: const std::string &s = "CLGaussianConditional\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override; }; -} \ No newline at end of file +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index 06df66112..74b967fc8 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -11,8 +11,7 @@ /** * @file HybridBayesTree.h - * @brief Hybrid Bayes Tree, the result of eliminating a - * HybridJunctionTree + * @brief Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree * @date Mar 11, 2022 * @author Fan Jiang */ @@ -22,8 +21,8 @@ #include #include #include -#include #include +#include #include @@ -39,22 +38,18 @@ class GTSAM_EXPORT HybridBayesTreeClique : 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() {} virtual ~HybridBayesTreeClique() {} - HybridBayesTreeClique( - const boost::shared_ptr& conditional) + HybridBayesTreeClique(const boost::shared_ptr& conditional) : Base(conditional) {} - }; /* ************************************************************************* */ /** A Bayes tree representing a Hybrid density */ -class GTSAM_EXPORT HybridBayesTree - : public BayesTree { +class GTSAM_EXPORT HybridBayesTree : public BayesTree { private: typedef BayesTree Base; diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index b3009d5b9..5cb98d921 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -1,6 +1,19 @@ -// -// Created by Fan Jiang on 3/11/22. -// +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file HybridConditional.cpp + * @date Mar 11, 2022 + * @author Fan Jiang + */ #include #include @@ -14,7 +27,10 @@ void HybridConditional::print(const std::string &s, const size_t contN = N - discreteKeys_.size(); while (index < N) { if (index > 0) { - if (index == nrFrontals_) std::cout << " | "; else std::cout << ", "; + if (index == nrFrontals_) + std::cout << " | "; + else + std::cout << ", "; } if (index < contN) { std::cout << formatter(keys()[index]); @@ -28,13 +44,12 @@ void HybridConditional::print(const std::string &s, if (inner) inner->print("", formatter); } -bool HybridConditional::equals(const HybridFactor &other, - double tol) const { +bool HybridConditional::equals(const HybridFactor &other, double tol) const { return false; } -HybridConditional HybridConditional::operator*(const HybridConditional &other) const { +HybridConditional HybridConditional::operator*( + const HybridConditional &other) const { return {}; } -} - +} // namespace gtsam diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index 92856cae2..89071092f 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -17,11 +17,10 @@ #pragma once -#include #include +#include #include - #include #include #include @@ -38,33 +37,30 @@ namespace gtsam { * - GaussianConditional */ class GTSAM_EXPORT HybridConditional -: public HybridFactor, -public Conditional { -public: + : public HybridFactor, + public Conditional { + public: // typedefs needed to play nice with gtsam - typedef HybridConditional This; ///< Typedef to this class + typedef HybridConditional This; ///< Typedef to this class typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class typedef HybridFactor BaseFactor; ///< Typedef to our factor base class typedef Conditional BaseConditional; ///< Typedef to our conditional base class -private: + private: // Type-erased pointer to the inner type std::unique_ptr inner; -public: -/// @name Standard Constructors -/// @{ + public: + /// @name Standard Constructors + /// @{ /// Default constructor needed for serialization. HybridConditional() = default; - HybridConditional(const KeyVector &continuousKeys, - const DiscreteKeys &discreteKeys, - size_t nFrontals) - : BaseFactor(continuousKeys, discreteKeys), BaseConditional(nFrontals) { - - } + HybridConditional(const KeyVector& continuousKeys, + const DiscreteKeys& discreteKeys, size_t nFrontals) + : BaseFactor(continuousKeys, discreteKeys), BaseConditional(nFrontals) {} /** * @brief Combine two conditionals, yielding a new conditional with the union @@ -83,9 +79,9 @@ public: */ HybridConditional operator*(const HybridConditional& other) const; -/// @} -/// @name Testable -/// @{ + /// @} + /// @name Testable + /// @{ /// GTSAM-style print void print( @@ -95,7 +91,7 @@ public: /// GTSAM-style equals bool equals(const HybridFactor& other, double tol = 1e-9) const override; -/// @} + /// @} }; // DiscreteConditional diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp index 1758e9025..96d3842b8 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.cpp +++ b/gtsam/hybrid/HybridDiscreteFactor.cpp @@ -1,6 +1,20 @@ -// -// Created by Fan Jiang on 3/11/22. -// +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file HybridDiscreteFactor.cpp + * @brief Wrapper for a discrete factor + * @date Mar 11, 2022 + * @author Fan Jiang + */ #include @@ -15,17 +29,16 @@ HybridDiscreteFactor::HybridDiscreteFactor(DiscreteFactor::shared_ptr other) HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf) : Base(dtf.discreteKeys()), - inner(boost::make_shared(std::move(dtf))) { - -} + inner(boost::make_shared(std::move(dtf))) {} bool HybridDiscreteFactor::equals(const HybridFactor &lf, double tol) const { return false; } -void HybridDiscreteFactor::print(const std::string &s, const KeyFormatter &formatter) const { +void HybridDiscreteFactor::print(const std::string &s, + const KeyFormatter &formatter) const { HybridFactor::print(s, formatter); inner->print("inner: ", formatter); }; -} \ No newline at end of file +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h index 9d574b736..09da9cf70 100644 --- a/gtsam/hybrid/HybridDiscreteFactor.h +++ b/gtsam/hybrid/HybridDiscreteFactor.h @@ -15,8 +15,8 @@ * @author Fan Jiang */ -#include #include +#include #include namespace gtsam { @@ -36,8 +36,10 @@ class HybridDiscreteFactor : public HybridFactor { HybridDiscreteFactor(DecisionTreeFactor &&dtf); public: - virtual bool equals(const HybridFactor& lf, double tol) const override; + virtual bool equals(const HybridFactor &lf, double tol) const override; - void print(const std::string &s = "HybridFactor\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override; + void print( + const std::string &s = "HybridFactor\n", + const KeyFormatter &formatter = DefaultKeyFormatter) const override; }; -} +} // namespace gtsam diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h index e218ce9f6..b72bbcad9 100644 --- a/gtsam/hybrid/HybridEliminationTree.h +++ b/gtsam/hybrid/HybridEliminationTree.h @@ -23,40 +23,39 @@ namespace gtsam { -class GTSAM_EXPORT HybridEliminationTree : - public EliminationTree -{ +class GTSAM_EXPORT HybridEliminationTree + : public EliminationTree { public: - typedef EliminationTree Base; ///< Base class - typedef HybridEliminationTree This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + typedef EliminationTree + Base; ///< Base class + typedef HybridEliminationTree This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class /** - * Build the elimination tree of a factor graph using pre-computed column structure. - * @param factorGraph The factor graph for which to build the elimination tree - * @param structure The set of factors involving each variable. If this is not - * precomputed, you can call the Create(const FactorGraph&) - * named constructor instead. - * @return The elimination tree - */ + * Build the elimination tree of a factor graph using pre-computed column + * structure. + * @param factorGraph The factor graph for which to build the elimination tree + * @param structure The set of factors involving each variable. If this is + * not precomputed, you can call the Create(const FactorGraph&) + * named constructor instead. + * @return The elimination tree + */ HybridEliminationTree(const HybridFactorGraph& factorGraph, - const VariableIndex& structure, const Ordering& order); + const VariableIndex& structure, const Ordering& order); - /** Build the elimination tree of a factor graph. Note that this has to compute the column - * structure as a VariableIndex, so if you already have this precomputed, use the other - * constructor instead. - * @param factorGraph The factor graph for which to build the elimination tree - */ + /** Build the elimination tree of a factor graph. Note that this has to + * compute the column structure as a VariableIndex, so if you already have + * this precomputed, use the other constructor instead. + * @param factorGraph The factor graph for which to build the elimination tree + */ HybridEliminationTree(const HybridFactorGraph& factorGraph, - const Ordering& order); + const Ordering& order); /** Test whether the tree is equal to another */ bool equals(const This& other, double tol = 1e-9) const; private: - friend class ::EliminationTreeTester; - }; -} +} // namespace gtsam diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp index 171ea790a..f16092eff 100644 --- a/gtsam/hybrid/HybridFactor.cpp +++ b/gtsam/hybrid/HybridFactor.cpp @@ -19,11 +19,12 @@ namespace gtsam { -KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys) { +KeyVector CollectKeys(const KeyVector &continuousKeys, + const DiscreteKeys &discreteKeys) { KeyVector allKeys; - std::copy(continuousKeys.begin(), continuousKeys.end(), std::back_inserter(allKeys)); - std::transform(discreteKeys.begin(), - discreteKeys.end(), + std::copy(continuousKeys.begin(), continuousKeys.end(), + std::back_inserter(allKeys)); + std::transform(discreteKeys.begin(), discreteKeys.end(), std::back_inserter(allKeys), [](const DiscreteKey &k) { return k.first; }); return allKeys; @@ -36,7 +37,8 @@ KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2) { return allKeys; } -DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, const DiscreteKeys &key2) { +DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, + const DiscreteKeys &key2) { DiscreteKeys allKeys; std::copy(key1.begin(), key1.end(), std::back_inserter(allKeys)); std::copy(key2.begin(), key2.end(), std::back_inserter(allKeys)); @@ -45,16 +47,20 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, const DiscreteKeys &k HybridFactor::HybridFactor() = default; -HybridFactor::HybridFactor(const KeyVector &keys) : Base(keys), isContinuous_(true) {} +HybridFactor::HybridFactor(const KeyVector &keys) + : Base(keys), isContinuous_(true) {} -HybridFactor::HybridFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys) - : Base( - CollectKeys(continuousKeys, discreteKeys)), isHybrid_(true), discreteKeys_(discreteKeys) {} +HybridFactor::HybridFactor(const KeyVector &continuousKeys, + const DiscreteKeys &discreteKeys) + : Base(CollectKeys(continuousKeys, discreteKeys)), + isHybrid_(true), + discreteKeys_(discreteKeys) {} -HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys) : Base(CollectKeys({}, discreteKeys)), - isDiscrete_(true), - discreteKeys_(discreteKeys) {} +HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys) + : Base(CollectKeys({}, discreteKeys)), + isDiscrete_(true), + discreteKeys_(discreteKeys) {} HybridFactor::~HybridFactor() = default; -} \ No newline at end of file +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index dd925fee2..cd562869e 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -17,29 +17,30 @@ #pragma once -#include -#include -#include #include +#include +#include +#include #include namespace gtsam { -KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys); +KeyVector CollectKeys(const KeyVector &continuousKeys, + const DiscreteKeys &discreteKeys); KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2); -DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, const DiscreteKeys &key2); +DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, + const DiscreteKeys &key2); /** * Base class for hybrid probabilistic factors */ class GTSAM_EXPORT HybridFactor : public Factor { - -public: - + public: // typedefs needed to play nice with gtsam - typedef HybridFactor This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class - typedef Factor Base; ///< Our base class + typedef HybridFactor This; ///< This class + typedef boost::shared_ptr + shared_ptr; ///< shared_ptr to this class + typedef Factor Base; ///< Our base class bool isDiscrete_ = false; bool isContinuous_ = false; @@ -47,31 +48,33 @@ public: DiscreteKeys discreteKeys_; -public: - -/// @name Standard Constructors -/// @{ + public: + /// @name Standard Constructors + /// @{ /** Default constructor creates empty factor */ HybridFactor(); - /** Construct from container of keys. This constructor is used internally from derived factor - * constructors, either from a container of keys or from a boost::assign::list_of. */ -// template -// HybridFactor(const CONTAINER &keys) : Base(keys) {} + /** Construct from container of keys. This constructor is used internally + * from derived factor + * constructors, either from a container of keys or from a + * boost::assign::list_of. */ + // template + // HybridFactor(const CONTAINER &keys) : Base(keys) {} explicit HybridFactor(const KeyVector &keys); - HybridFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys); + HybridFactor(const KeyVector &continuousKeys, + const DiscreteKeys &discreteKeys); explicit HybridFactor(const DiscreteKeys &discreteKeys); /// Virtual destructor virtual ~HybridFactor(); -/// @} -/// @name Testable -/// @{ + /// @} + /// @name Testable + /// @{ /// equals virtual bool equals(const HybridFactor &lf, double tol = 1e-9) const = 0; @@ -87,16 +90,16 @@ public: this->printKeys("", formatter); } -/// @} -/// @name Standard Interface -/// @{ + /// @} + /// @name Standard Interface + /// @{ -/// @} + /// @} }; // HybridFactor // traits -template<> +template <> struct traits : public Testable {}; -}// namespace gtsam +} // namespace gtsam diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index 03610166e..7d4daaceb 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -1,25 +1,37 @@ -// -// Created by Fan Jiang on 3/11/22. -// +/* ---------------------------------------------------------------------------- -#include "gtsam/inference/Key.h" -#include -#include -#include -#include + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file HybridFactorGraph.cpp + * @brief Hybrid factor graph that uses type erasure + * @author Fan Jiang + * @date Mar 11, 2022 + */ -#include #include - +#include +#include +#include +#include +#include #include #include #include +#include "gtsam/inference/Key.h" + namespace gtsam { -template -class EliminateableFactorGraph; +template class EliminateableFactorGraph; static std::string BLACK_BOLD = "\033[1;30m"; static std::string RED_BOLD = "\033[1;31m"; @@ -29,8 +41,7 @@ static std::string RESET = "\033[0m"; /* ************************************************************************ */ std::pair // -EliminateHybrid(const HybridFactorGraph &factors, - const Ordering &frontalKeys) { +EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) { // NOTE(fan): Because we are in the Conditional Gaussian regime there are only // a few cases: // continuous variable, we make a GM if there are hybrid factors; @@ -38,9 +49,10 @@ EliminateHybrid(const HybridFactorGraph &factors, // discrete variable, no continuous factor is allowed (escapes CG regime), so // we panic, if discrete only we do the discrete elimination. - // However it is not that simple. During elimination it is possible that the multifrontal needs - // to eliminate an ordering that contains both Gaussian and hybrid variables, for example x1, c1. - // In this scenario, we will have a density P(x1, c1) that is a CLG P(x1|c1)P(c1) (see Murphy02) + // However it is not that simple. During elimination it is possible that the + // multifrontal needs to eliminate an ordering that contains both Gaussian and + // hybrid variables, for example x1, c1. In this scenario, we will have a + // density P(x1, c1) that is a CLG P(x1|c1)P(c1) (see Murphy02) // The issue here is that, how can we know which variable is discrete if we // unify Values? Obviously we can tell using the factors, but is that fast? @@ -102,7 +114,8 @@ EliminateHybrid(const HybridFactorGraph &factors, for (auto &f : frontalKeys) { if (discreteCardinalities.find(f) != discreteCardinalities.end()) { auto &key = discreteCardinalities.at(f); - std::cout << boost::format(" (%1%,%2%),") % DefaultKeyFormatter(key.first) % key.second; + std::cout << boost::format(" (%1%,%2%),") % + DefaultKeyFormatter(key.first) % key.second; } else { std::cout << " " << DefaultKeyFormatter(f) << ","; } @@ -115,7 +128,8 @@ EliminateHybrid(const HybridFactorGraph &factors, for (auto &f : separatorKeys) { if (discreteCardinalities.find(f) != discreteCardinalities.end()) { auto &key = discreteCardinalities.at(f); - std::cout << boost::format(" (%1%,%2%),") % DefaultKeyFormatter(key.first) % key.second; + std::cout << boost::format(" (%1%,%2%),") % + DefaultKeyFormatter(key.first) % key.second; } else { std::cout << DefaultKeyFormatter(f) << ","; } @@ -124,9 +138,10 @@ EliminateHybrid(const HybridFactorGraph &factors, // PRODUCT: multiply all factors gttic(product); - HybridConditional sum(KeyVector(continuousSeparator.begin(), continuousSeparator.end()), - DiscreteKeys(discreteSeparator.begin(), discreteSeparator.end()), - separatorKeys.size()); + HybridConditional sum( + KeyVector(continuousSeparator.begin(), continuousSeparator.end()), + DiscreteKeys(discreteSeparator.begin(), discreteSeparator.end()), + separatorKeys.size()); // HybridDiscreteFactor product(DiscreteConditional()); // for (auto&& factor : factors) product = (*factor) * product; @@ -139,10 +154,8 @@ EliminateHybrid(const HybridFactorGraph &factors, // Ordering keys for the conditional so that frontalKeys are really in front Ordering orderedKeys; - orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(), - frontalKeys.end()); - orderedKeys.insert(orderedKeys.end(), sum.keys().begin(), - sum.keys().end()); + orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(), frontalKeys.end()); + orderedKeys.insert(orderedKeys.end(), sum.keys().begin(), sum.keys().end()); // now divide product/sum to get conditional gttic(divide); @@ -163,12 +176,11 @@ EliminateHybrid(const HybridFactorGraph &factors, std::cout << RED_BOLD << "[End Eliminate]\n" << RESET; // return std::make_pair(conditional, sum); - return std::make_pair( - conditional, - boost::make_shared(std::move(sum))); + return std::make_pair(conditional, + boost::make_shared(std::move(sum))); } void HybridFactorGraph::add(JacobianFactor &&factor) { FactorGraph::add(boost::make_shared(std::move(factor))); } -} +} // namespace gtsam diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h index 9a57d36e6..92f98c8f2 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/HybridFactorGraph.h @@ -36,43 +36,50 @@ class HybridJunctionTree; class JacobianFactor; /** Main elimination function for HybridFactorGraph */ -GTSAM_EXPORT std::pair, HybridFactor::shared_ptr> -EliminateHybrid(const HybridFactorGraph& factors, const Ordering& keys); +GTSAM_EXPORT + std::pair, HybridFactor::shared_ptr> + EliminateHybrid(const HybridFactorGraph& factors, const Ordering& keys); /* ************************************************************************* */ -template<> struct EliminationTraits -{ - typedef HybridFactor FactorType; ///< Type of factors in factor graph - typedef HybridFactorGraph FactorGraphType; ///< Type of the factor graph (e.g. HybridFactorGraph) - 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 +template <> +struct EliminationTraits { + typedef HybridFactor FactorType; ///< Type of factors in factor graph + typedef HybridFactorGraph + FactorGraphType; ///< Type of the factor graph (e.g. HybridFactorGraph) + 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 /// The default dense elimination function - static std::pair, boost::shared_ptr > + static std::pair, + boost::shared_ptr > DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) { - return EliminateHybrid(factors, keys); } + return EliminateHybrid(factors, keys); + } }; - -class HybridFactorGraph : public FactorGraph, public EliminateableFactorGraph { +class HybridFactorGraph : public FactorGraph, + public EliminateableFactorGraph { public: using Base = FactorGraph; - using This = HybridFactorGraph; ///< this class + using This = HybridFactorGraph; ///< this class using BaseEliminateable = - EliminateableFactorGraph; ///< for elimination + EliminateableFactorGraph; ///< for elimination using shared_ptr = boost::shared_ptr; ///< shared_ptr to This using Values = gtsam::Values; ///< backwards compatibility - using Indices = KeyVector; ///> map from keys to values + using Indices = KeyVector; ///> map from keys to values public: HybridFactorGraph() = default; -// /** Construct from container of factors (shared_ptr or plain objects) */ -// template -// explicit HybridFactorGraph(const CONTAINER& factors) : Base(factors) {} + // /** Construct from container of factors (shared_ptr or plain objects) */ + // template + // explicit HybridFactorGraph(const CONTAINER& factors) : Base(factors) {} /** Implicit copy/downcast constructor to override explicit template container * constructor. In BayesTree this is used for: @@ -84,8 +91,7 @@ class HybridFactorGraph : public FactorGraph, public Eliminateable using FactorGraph::add; /// Add a factor directly using a shared_ptr. - void add(JacobianFactor &&factor); + void add(JacobianFactor&& factor); }; -} - +} // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index faa4ba998..d83f90024 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -1,6 +1,19 @@ -// -// Created by Fan Jiang on 3/11/22. -// +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file HybridGaussianFactor.cpp + * @date Mar 11, 2022 + * @author Fan Jiang + */ #include @@ -8,20 +21,22 @@ namespace gtsam { -HybridGaussianFactor::HybridGaussianFactor(GaussianFactor::shared_ptr other) : Base(other->keys()) { +HybridGaussianFactor::HybridGaussianFactor(GaussianFactor::shared_ptr other) + : Base(other->keys()) { inner = other; } -HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf) : Base(jf.keys()), inner(boost::make_shared(std::move(jf))) { +HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf) + : Base(jf.keys()), + inner(boost::make_shared(std::move(jf))) {} -} - -bool HybridGaussianFactor::equals(const HybridFactor& lf, double tol) const { +bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const { return false; } -void HybridGaussianFactor::print(const std::string &s, const KeyFormatter &formatter) const { +void HybridGaussianFactor::print(const std::string &s, + const KeyFormatter &formatter) const { HybridFactor::print(s, formatter); inner->print("inner: ", formatter); }; -} \ No newline at end of file +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 8562075b4..23a3c0110 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -15,9 +15,9 @@ * @author Fan Jiang */ +#include #include #include -#include namespace gtsam { @@ -36,8 +36,10 @@ class HybridGaussianFactor : public HybridFactor { explicit HybridGaussianFactor(JacobianFactor &&jf); public: - virtual bool equals(const HybridFactor& lf, double tol) const override; + virtual bool equals(const HybridFactor &lf, double tol) const override; - void print(const std::string &s = "HybridFactor\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override; + void print( + const std::string &s = "HybridFactor\n", + const KeyFormatter &formatter = DefaultKeyFormatter) const override; }; -} +} // namespace gtsam diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index 0e3d2ea00..9da1bfed3 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -15,9 +15,9 @@ * @author Fan Jiang */ -#include -#include #include +#include +#include namespace gtsam { @@ -27,7 +27,7 @@ template class JunctionTree; /* ************************************************************************* */ HybridJunctionTree::HybridJunctionTree( - const HybridEliminationTree& eliminationTree) : - Base(eliminationTree) {} + const HybridEliminationTree& eliminationTree) + : Base(eliminationTree) {} -} +} // namespace gtsam diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp index c682f5216..f945d0702 100644 --- a/gtsam/hybrid/tests/testHybridConditional.cpp +++ b/gtsam/hybrid/tests/testHybridConditional.cpp @@ -15,32 +15,30 @@ * @author Fan Jiang */ -#include "Test.h" -#include -#include -#include -#include -#include -#include +#include +#include #include +#include #include #include - -#include +#include +#include +#include +#include +#include #include - #include - -#include +#include #include + using namespace boost::assign; using namespace std; using namespace gtsam; -using gtsam::symbol_shorthand::X; using gtsam::symbol_shorthand::C; +using gtsam::symbol_shorthand::X; /* ************************************************************************* */ TEST(HybridFactorGraph, creation) { @@ -51,17 +49,13 @@ TEST(HybridFactorGraph, creation) { hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1))); CLGaussianConditional clgc( - {X(0)}, {X(1)}, - DiscreteKeys(DiscreteKey{C(0), 2}), + {X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}), CLGaussianConditional::Conditionals( C(0), - boost::make_shared( - X(0), Z_3x1, I_3x3, X(1), I_3x3), - boost::make_shared( - X(0), Vector3::Ones(), - I_3x3, X(1), - I_3x3)) - ); + boost::make_shared(X(0), Z_3x1, I_3x3, X(1), + I_3x3), + boost::make_shared(X(0), Vector3::Ones(), I_3x3, + X(1), I_3x3))); GTSAM_PRINT(clgc); } @@ -90,7 +84,6 @@ TEST_DISABLED(HybridFactorGraph, eliminateMultifrontal) { } TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalSimple) { - std::cout << ">>>>>>>>>>>>>>\n"; HybridFactorGraph hfg; @@ -100,29 +93,27 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalSimple) { hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); - DecisionTree dt(C(1), - boost::make_shared(X(1), - I_3x3, - Z_3x1), - boost::make_shared(X(1), - I_3x3, - Vector3::Ones())); + DecisionTree dt( + C(1), boost::make_shared(X(1), I_3x3, Z_3x1), + boost::make_shared(X(1), I_3x3, Vector3::Ones())); hfg.add(CGMixtureFactor({X(1)}, {c1}, dt)); hfg.add(CGMixtureFactor({X(0)}, {c1}, dt)); hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c1, {2, 8}))); - hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4"))); - // hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(2), 2}, {C(3), 2}}, "1 2 3 4"))); - // hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(3), 2}, {C(1), 2}}, "1 2 2 1"))); + hfg.add(HybridDiscreteFactor( + DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4"))); + // hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(2), 2}, {C(3), 2}}, "1 + // 2 3 4"))); hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(3), 2}, + // {C(1), 2}}, "1 2 2 1"))); - auto result = hfg.eliminateMultifrontal(Ordering::ColamdConstrainedLast(hfg, {C(1), C(2)})); + auto result = hfg.eliminateMultifrontal( + Ordering::ColamdConstrainedLast(hfg, {C(1), C(2)})); GTSAM_PRINT(*result); GTSAM_PRINT(*result->marginalFactor(C(1))); } TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) { - std::cout << ">>>>>>>>>>>>>>\n"; HybridFactorGraph hfg; @@ -132,29 +123,28 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) { hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); - DecisionTree dt(C(1), - boost::make_shared(X(1), - I_3x3, - Z_3x1), - boost::make_shared(X(1), - I_3x3, - Vector3::Ones())); + DecisionTree dt( + C(1), boost::make_shared(X(1), I_3x3, Z_3x1), + boost::make_shared(X(1), I_3x3, Vector3::Ones())); hfg.add(CGMixtureFactor({X(1)}, {c}, dt)); hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8}))); -// hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4"))); + // hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 + // 2 3 4"))); - auto result = hfg.eliminateMultifrontal(Ordering::ColamdConstrainedLast(hfg, {C(1)})); + auto result = + hfg.eliminateMultifrontal(Ordering::ColamdConstrainedLast(hfg, {C(1)})); GTSAM_PRINT(*result); // We immediately need to escape the CLG domain if we do this!!! GTSAM_PRINT(*result->marginalFactor(X(1))); /* - Explanation: the Junction tree will need to reeliminate to get to the marginal on X(1), which - is not possible because it involves eliminating discrete before continuous. The solution to this, - however, is in Murphy02. TLDR is that this is 1. expensive and 2. inexact. neverless it is doable. - And I believe that we should do this. + Explanation: the Junction tree will need to reeliminate to get to the marginal + on X(1), which is not possible because it involves eliminating discrete before + continuous. The solution to this, however, is in Murphy02. TLDR is that this + is 1. expensive and 2. inexact. neverless it is doable. And I believe that we + should do this. */ } @@ -164,4 +154,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ -