diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 4f8d2350c..14a54c0b4 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -182,8 +182,10 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) { for (auto &&conditional : *this) { if (auto gm = conditional->asMixture()) { // Make a copy of the Gaussian mixture and prune it! - auto prunedHybridGaussianConditional = std::make_shared(*gm); - prunedHybridGaussianConditional->prune(prunedDiscreteProbs); // imperative :-( + auto prunedHybridGaussianConditional = + std::make_shared(*gm); + prunedHybridGaussianConditional->prune( + prunedDiscreteProbs); // imperative :-( // Type-erase and add to the pruned Bayes Net fragment. prunedBayesNetFragment.push_back(prunedHybridGaussianConditional); diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index 5ef74ea7d..8a8511aef 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -157,10 +157,10 @@ double HybridConditional::logNormalizationConstant() const { return gc->logNormalizationConstant(); } if (auto gm = asMixture()) { - return gm->logNormalizationConstant(); // 0.0! + return gm->logNormalizationConstant(); // 0.0! } if (auto dc = asDiscrete()) { - return dc->logNormalizationConstant(); // 0.0! + return dc->logNormalizationConstant(); // 0.0! } throw std::runtime_error( "HybridConditional::logProbability: conditional type not handled"); diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index d02b69127..fb51e95f6 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -18,8 +18,8 @@ #pragma once #include -#include #include +#include #include #include #include @@ -127,7 +127,8 @@ class GTSAM_EXPORT HybridConditional * @param gaussianMixture Gaussian Mixture Conditional used to create the * HybridConditional. */ - HybridConditional(const std::shared_ptr& gaussianMixture); + HybridConditional( + const std::shared_ptr& gaussianMixture); /// @} /// @name Testable @@ -222,8 +223,10 @@ class GTSAM_EXPORT HybridConditional boost::serialization::void_cast_register( static_cast(NULL), static_cast(NULL)); } else { - boost::serialization::void_cast_register( - static_cast(NULL), static_cast(NULL)); + boost::serialization::void_cast_register( + static_cast(NULL), + static_cast(NULL)); } } #endif diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h index e1eb1764a..543e09c6f 100644 --- a/gtsam/hybrid/HybridEliminationTree.h +++ b/gtsam/hybrid/HybridEliminationTree.h @@ -35,8 +35,8 @@ class GTSAM_EXPORT HybridEliminationTree public: typedef EliminationTree - Base; ///< Base class - typedef HybridEliminationTree This; ///< This class + Base; ///< Base class + typedef HybridEliminationTree This; ///< This class typedef std::shared_ptr shared_ptr; ///< Shared pointer to this class /// @name Constructors diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index 4ad85b827..42e702155 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -50,7 +50,8 @@ HybridGaussianConditional::HybridGaussianConditional( } /* *******************************************************************************/ -const HybridGaussianConditional::Conditionals &HybridGaussianConditional::conditionals() const { +const HybridGaussianConditional::Conditionals & +HybridGaussianConditional::conditionals() const { return conditionals_; } @@ -59,20 +60,22 @@ HybridGaussianConditional::HybridGaussianConditional( KeyVector &&continuousFrontals, KeyVector &&continuousParents, DiscreteKeys &&discreteParents, std::vector &&conditionals) - : HybridGaussianConditional(continuousFrontals, continuousParents, discreteParents, - Conditionals(discreteParents, conditionals)) {} + : HybridGaussianConditional(continuousFrontals, continuousParents, + discreteParents, + Conditionals(discreteParents, conditionals)) {} /* *******************************************************************************/ HybridGaussianConditional::HybridGaussianConditional( const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, const std::vector &conditionals) - : HybridGaussianConditional(continuousFrontals, continuousParents, discreteParents, - Conditionals(discreteParents, conditionals)) {} + : HybridGaussianConditional(continuousFrontals, continuousParents, + discreteParents, + Conditionals(discreteParents, conditionals)) {} /* *******************************************************************************/ -// TODO(dellaert): This is copy/paste: HybridGaussianConditional should be derived from -// HybridGaussianFactor, no? +// TODO(dellaert): This is copy/paste: HybridGaussianConditional should be +// derived from HybridGaussianFactor, no? GaussianFactorGraphTree HybridGaussianConditional::add( const GaussianFactorGraphTree &sum) const { using Y = GaussianFactorGraph; @@ -86,7 +89,8 @@ GaussianFactorGraphTree HybridGaussianConditional::add( } /* *******************************************************************************/ -GaussianFactorGraphTree HybridGaussianConditional::asGaussianFactorGraphTree() const { +GaussianFactorGraphTree HybridGaussianConditional::asGaussianFactorGraphTree() + const { auto wrap = [this](const GaussianConditional::shared_ptr &gc) { // First check if conditional has not been pruned if (gc) { @@ -131,7 +135,8 @@ GaussianConditional::shared_ptr HybridGaussianConditional::operator()( } /* *******************************************************************************/ -bool HybridGaussianConditional::equals(const HybridFactor &lf, double tol) const { +bool HybridGaussianConditional::equals(const HybridFactor &lf, + double tol) const { const This *e = dynamic_cast(&lf); if (e == nullptr) return false; @@ -150,7 +155,7 @@ bool HybridGaussianConditional::equals(const HybridFactor &lf, double tol) const /* *******************************************************************************/ void HybridGaussianConditional::print(const std::string &s, - const KeyFormatter &formatter) const { + const KeyFormatter &formatter) const { std::cout << (s.empty() ? "" : s + "\n"); if (isContinuous()) std::cout << "Continuous "; if (isDiscrete()) std::cout << "Discrete "; @@ -193,7 +198,8 @@ KeyVector HybridGaussianConditional::continuousParents() const { } /* ************************************************************************* */ -bool HybridGaussianConditional::allFrontalsGiven(const VectorValues &given) const { +bool HybridGaussianConditional::allFrontalsGiven( + const VectorValues &given) const { for (auto &&kv : given) { if (given.find(kv.first) == given.end()) { return false; @@ -207,7 +213,8 @@ std::shared_ptr HybridGaussianConditional::likelihood( const VectorValues &given) const { if (!allFrontalsGiven(given)) { throw std::runtime_error( - "HybridGaussianConditional::likelihood: given values are missing some frontals."); + "HybridGaussianConditional::likelihood: given values are missing some " + "frontals."); } const DiscreteKeys discreteParentKeys = discreteKeys(); @@ -365,7 +372,8 @@ double HybridGaussianConditional::error(const HybridValues &values) const { } /* *******************************************************************************/ -double HybridGaussianConditional::logProbability(const HybridValues &values) const { +double HybridGaussianConditional::logProbability( + const HybridValues &values) const { auto conditional = conditionals_(values.discrete()); return conditional->logProbability(values.continuous()); } diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index 87ddb2cb8..fc315c6f9 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -23,8 +23,8 @@ #include #include #include -#include #include +#include #include #include @@ -102,9 +102,9 @@ class GTSAM_EXPORT HybridGaussianConditional * discreteParents will be used as the labels in the decision tree. */ HybridGaussianConditional(const KeyVector &continuousFrontals, - const KeyVector &continuousParents, - const DiscreteKeys &discreteParents, - const Conditionals &conditionals); + const KeyVector &continuousParents, + const DiscreteKeys &discreteParents, + const Conditionals &conditionals); /** * @brief Make a Gaussian Mixture from a list of Gaussian conditionals @@ -114,9 +114,10 @@ class GTSAM_EXPORT HybridGaussianConditional * @param discreteParents Discrete parents variables * @param conditionals List of conditionals */ - HybridGaussianConditional(KeyVector &&continuousFrontals, KeyVector &&continuousParents, - DiscreteKeys &&discreteParents, - std::vector &&conditionals); + HybridGaussianConditional( + KeyVector &&continuousFrontals, KeyVector &&continuousParents, + DiscreteKeys &&discreteParents, + std::vector &&conditionals); /** * @brief Make a Gaussian Mixture from a list of Gaussian conditionals @@ -277,6 +278,7 @@ std::set DiscreteKeysAsSet(const DiscreteKeys &discreteKeys); // traits template <> -struct traits : public Testable {}; +struct traits + : public Testable {}; } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index 4445d875f..f7207bf81 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -30,8 +30,8 @@ namespace gtsam { /* *******************************************************************************/ HybridGaussianFactor::HybridGaussianFactor(const KeyVector &continuousKeys, - const DiscreteKeys &discreteKeys, - const Factors &factors) + const DiscreteKeys &discreteKeys, + const Factors &factors) : Base(continuousKeys, discreteKeys), factors_(factors) {} /* *******************************************************************************/ @@ -53,7 +53,7 @@ bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const { /* *******************************************************************************/ void HybridGaussianFactor::print(const std::string &s, - const KeyFormatter &formatter) const { + const KeyFormatter &formatter) const { std::cout << (s.empty() ? "" : s + "\n"); std::cout << "HybridGaussianFactor" << std::endl; HybridFactor::print("", formatter); diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 9915e24e6..650f7a5a7 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -84,8 +84,8 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { * as the mixture density. */ HybridGaussianFactor(const KeyVector &continuousKeys, - const DiscreteKeys &discreteKeys, - const Factors &factors); + const DiscreteKeys &discreteKeys, + const Factors &factors); /** * @brief Construct a new HybridGaussianFactor object using a vector of @@ -96,10 +96,10 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { * @param factors Vector of gaussian factor shared pointers. */ HybridGaussianFactor(const KeyVector &continuousKeys, - const DiscreteKeys &discreteKeys, - const std::vector &factors) + const DiscreteKeys &discreteKeys, + const std::vector &factors) : HybridGaussianFactor(continuousKeys, discreteKeys, - Factors(discreteKeys, factors)) {} + Factors(discreteKeys, factors)) {} /// @} /// @name Testable @@ -168,7 +168,6 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { // traits template <> -struct traits : public Testable { -}; +struct traits : public Testable {}; } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 4c6546708..900ec90c7 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -23,11 +23,11 @@ #include #include #include -#include -#include #include #include #include +#include +#include #include #include #include @@ -363,7 +363,7 @@ static std::shared_ptr createHybridGaussianFactor( correct); return std::make_shared(continuousSeparator, - discreteSeparator, newFactors); + discreteSeparator, newFactors); } static std::pair> @@ -406,7 +406,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, continuousSeparator.empty() ? createDiscreteFactor(eliminationResults, discreteSeparator) : createHybridGaussianFactor(eliminationResults, continuousSeparator, - discreteSeparator); + discreteSeparator); // Create the HybridGaussianConditional from the conditionals HybridGaussianConditional::Conditionals conditionals( diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index e6ce4467c..911058437 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -18,9 +18,9 @@ #pragma once -#include #include #include +#include #include #include #include @@ -221,7 +221,6 @@ class GTSAM_EXPORT HybridGaussianFactorGraph /// Get the GaussianFactorGraph at a given discrete assignment. GaussianFactorGraph operator()(const DiscreteValues& assignment) const; - }; } // namespace gtsam diff --git a/gtsam/hybrid/HybridJunctionTree.h b/gtsam/hybrid/HybridJunctionTree.h index a197e6e3c..17b4649fc 100644 --- a/gtsam/hybrid/HybridJunctionTree.h +++ b/gtsam/hybrid/HybridJunctionTree.h @@ -51,11 +51,10 @@ class HybridEliminationTree; */ class GTSAM_EXPORT HybridJunctionTree : public JunctionTree { - public: typedef JunctionTree - Base; ///< Base class - typedef HybridJunctionTree This; ///< This class + Base; ///< Base class + typedef HybridJunctionTree This; ///< This class typedef std::shared_ptr shared_ptr; ///< Shared pointer to this class /** diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h index 7b2990cb0..f5d9cd356 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.h +++ b/gtsam/hybrid/HybridNonlinearFactor.h @@ -75,7 +75,7 @@ class HybridNonlinearFactor : public HybridFactor { * normalized. */ HybridNonlinearFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys, - const Factors& factors, bool normalized = false) + const Factors& factors, bool normalized = false) : Base(keys, discreteKeys), factors_(factors), normalized_(normalized) {} /** @@ -96,8 +96,8 @@ class HybridNonlinearFactor : public HybridFactor { */ template HybridNonlinearFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys, - const std::vector>& factors, - bool normalized = false) + const std::vector>& factors, + bool normalized = false) : Base(keys, discreteKeys), normalized_(normalized) { std::vector nonlinear_factors; KeySet continuous_keys_set(keys.begin(), keys.end()); @@ -201,13 +201,14 @@ class HybridNonlinearFactor : public HybridFactor { /// Check equality bool equals(const HybridFactor& other, double tol = 1e-9) const override { - // We attempt a dynamic cast from HybridFactor to HybridNonlinearFactor. If it - // fails, return false. + // We attempt a dynamic cast from HybridFactor to HybridNonlinearFactor. If + // it fails, return false. if (!dynamic_cast(&other)) return false; - // If the cast is successful, we'll properly construct a HybridNonlinearFactor - // object from `other` - const HybridNonlinearFactor& f(static_cast(other)); + // If the cast is successful, we'll properly construct a + // HybridNonlinearFactor object from `other` + const HybridNonlinearFactor& f( + static_cast(other)); // Ensure that this HybridNonlinearFactor and `f` have the same `factors_`. auto compare = [tol](const sharedFactor& a, const sharedFactor& b) { diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp index 76703ad2d..78370a157 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp @@ -20,8 +20,8 @@ #include #include #include -#include #include +#include #include namespace gtsam { @@ -80,7 +80,8 @@ void HybridNonlinearFactorGraph::printErrors( gmf->errorTree(values.continuous()).print("", keyFormatter); std::cout << std::endl; } - } else if (auto gm = std::dynamic_pointer_cast(factor)) { + } else if (auto gm = std::dynamic_pointer_cast( + factor)) { if (factor == nullptr) { std::cout << "nullptr" << "\n"; diff --git a/gtsam/hybrid/HybridNonlinearISAM.h b/gtsam/hybrid/HybridNonlinearISAM.h index 3372593be..e41b4ebe1 100644 --- a/gtsam/hybrid/HybridNonlinearISAM.h +++ b/gtsam/hybrid/HybridNonlinearISAM.h @@ -19,6 +19,7 @@ #include #include + #include namespace gtsam { diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 834a067c3..686ce60ac 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -10,26 +10,26 @@ class HybridValues { gtsam::DiscreteValues discrete() const; HybridValues(); - HybridValues(const gtsam::VectorValues &cv, const gtsam::DiscreteValues &dv); + HybridValues(const gtsam::VectorValues& cv, const gtsam::DiscreteValues& dv); void print(string s = "HybridValues", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::HybridValues& other, double tol) const; - + void insert(gtsam::Key j, int value); void insert(gtsam::Key j, const gtsam::Vector& value); - + void insert_or_assign(gtsam::Key j, const gtsam::Vector& value); void insert_or_assign(gtsam::Key j, size_t value); void insert(const gtsam::VectorValues& values); void insert(const gtsam::DiscreteValues& values); void insert(const gtsam::HybridValues& values); - + void update(const gtsam::VectorValues& values); void update(const gtsam::DiscreteValues& values); void update(const gtsam::HybridValues& values); - + size_t& atDiscrete(gtsam::Key j); gtsam::Vector& at(gtsam::Key j); }; @@ -42,7 +42,7 @@ virtual class HybridFactor : gtsam::Factor { bool equals(const gtsam::HybridFactor& other, double tol = 1e-9) const; // Standard interface: - double error(const gtsam::HybridValues &values) const; + double error(const gtsam::HybridValues& values) const; bool isDiscrete() const; bool isContinuous() const; bool isHybrid() const; @@ -86,11 +86,12 @@ class HybridGaussianFactor : gtsam::HybridFactor { #include class HybridGaussianConditional : gtsam::HybridFactor { - HybridGaussianConditional(const gtsam::KeyVector& continuousFrontals, - const gtsam::KeyVector& continuousParents, - const gtsam::DiscreteKeys& discreteParents, - const std::vector& - conditionalsList); + HybridGaussianConditional( + const gtsam::KeyVector& continuousFrontals, + const gtsam::KeyVector& continuousParents, + const gtsam::DiscreteKeys& discreteParents, + const std::vector& + conditionalsList); gtsam::HybridGaussianFactor* likelihood( const gtsam::VectorValues& frontals) const; @@ -139,7 +140,7 @@ class HybridBayesNet { size_t size() const; gtsam::KeySet keys() const; const gtsam::HybridConditional* at(size_t i) const; - + // Standard interface: double logProbability(const gtsam::HybridValues& values) const; double evaluate(const gtsam::HybridValues& values) const; @@ -149,7 +150,7 @@ class HybridBayesNet { const gtsam::VectorValues& measurements) const; gtsam::HybridValues optimize() const; - gtsam::HybridValues sample(const gtsam::HybridValues &given) const; + gtsam::HybridValues sample(const gtsam::HybridValues& given) const; gtsam::HybridValues sample() const; void print(string s = "HybridBayesNet\n", @@ -189,7 +190,8 @@ class HybridGaussianFactorGraph { const gtsam::HybridFactor* at(size_t i) const; void print(string s = "") const; - bool equals(const gtsam::HybridGaussianFactorGraph& fg, double tol = 1e-9) const; + bool equals(const gtsam::HybridGaussianFactorGraph& fg, + double tol = 1e-9) const; // evaluation double error(const gtsam::HybridValues& values) const; @@ -222,7 +224,8 @@ class HybridNonlinearFactorGraph { void push_back(gtsam::HybridFactor* factor); void push_back(gtsam::NonlinearFactor* factor); void push_back(gtsam::DiscreteFactor* factor); - gtsam::HybridGaussianFactorGraph linearize(const gtsam::Values& continuousValues) const; + gtsam::HybridGaussianFactorGraph linearize( + const gtsam::Values& continuousValues) const; bool empty() const; void remove(size_t i); @@ -231,8 +234,8 @@ class HybridNonlinearFactorGraph { const gtsam::HybridFactor* at(size_t i) const; void print(string s = "HybridNonlinearFactorGraph\n", - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; }; #include @@ -243,18 +246,18 @@ class HybridNonlinearFactor : gtsam::HybridFactor { bool normalized = false); template - HybridNonlinearFactor(const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys, - const std::vector& factors, - bool normalized = false); + HybridNonlinearFactor(const gtsam::KeyVector& keys, + const gtsam::DiscreteKeys& discreteKeys, + const std::vector& factors, + bool normalized = false); double error(const gtsam::Values& continuousValues, const gtsam::DiscreteValues& discreteValues) const; - double nonlinearFactorLogNormalizingConstant(const gtsam::NonlinearFactor* factor, - const gtsam::Values& values) const; + double nonlinearFactorLogNormalizingConstant( + const gtsam::NonlinearFactor* factor, const gtsam::Values& values) const; - HybridGaussianFactor* linearize( - const gtsam::Values& continuousValues) const; + HybridGaussianFactor* linearize(const gtsam::Values& continuousValues) const; void print(string s = "HybridNonlinearFactor\n", const gtsam::KeyFormatter& keyFormatter = diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 0e8680aa9..f08d29fd5 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -21,8 +21,8 @@ #include #include #include -#include #include +#include #include #include #include @@ -60,9 +60,9 @@ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain( hfg.add(HybridGaussianFactor( {keyFunc(t), keyFunc(t + 1)}, {{dKeyFunc(t), 2}}, {std::make_shared(keyFunc(t), I_3x3, keyFunc(t + 1), - I_3x3, Z_3x1), + I_3x3, Z_3x1), std::make_shared(keyFunc(t), I_3x3, keyFunc(t + 1), - I_3x3, Vector3::Ones())})); + I_3x3, Vector3::Ones())})); if (t > 1) { hfg.add(DecisionTreeFactor({{dKeyFunc(t - 1), 2}, {dKeyFunc(t), 2}}, diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 299562e32..308daca9a 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -19,10 +19,10 @@ #include #include #include +#include #include #include #include -#include #include #include #include diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp index f9d276760..847a38fa9 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp @@ -20,9 +20,9 @@ #include #include +#include #include #include -#include #include #include #include @@ -229,8 +229,8 @@ static HybridBayesNet GetGaussianMixtureModel(double mu0, double mu1, c1 = make_shared(z, Vector1(mu1), I_1x1, model1); HybridBayesNet hbn; - hbn.emplace_shared(KeyVector{z}, KeyVector{}, - DiscreteKeys{m}, std::vector{c0, c1}); + hbn.emplace_shared( + KeyVector{z}, KeyVector{}, DiscreteKeys{m}, std::vector{c0, c1}); auto mixing = make_shared(m, "0.5/0.5"); hbn.push_back(mixing); diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 3386daac8..acd7b280f 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -21,12 +21,12 @@ #include #include #include -#include -#include #include #include #include #include +#include +#include #include #include #include @@ -71,13 +71,14 @@ TEST(HybridGaussianFactorGraph, Creation) { // Define a gaussian mixture conditional P(x0|x1, c0) and add it to the factor // graph - HybridGaussianConditional gm({X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{M(0), 2}), - HybridGaussianConditional::Conditionals( - M(0), - std::make_shared( - X(0), Z_3x1, I_3x3, X(1), I_3x3), - std::make_shared( - X(0), Vector3::Ones(), I_3x3, X(1), I_3x3))); + HybridGaussianConditional gm( + {X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{M(0), 2}), + HybridGaussianConditional::Conditionals( + M(0), + std::make_shared(X(0), Z_3x1, I_3x3, X(1), + I_3x3), + std::make_shared(X(0), Vector3::Ones(), I_3x3, + X(1), I_3x3))); hfg.add(gm); EXPECT_LONGS_EQUAL(2, hfg.size()); @@ -681,8 +682,8 @@ TEST(HybridGaussianFactorGraph, ErrorTreeWithConditional) { x0, -I_1x1, model0), c1 = make_shared(f01, Vector1(mu), I_1x1, x1, I_1x1, x0, -I_1x1, model1); - hbn.emplace_shared(KeyVector{f01}, KeyVector{x0, x1}, - DiscreteKeys{m1}, std::vector{c0, c1}); + hbn.emplace_shared( + KeyVector{f01}, KeyVector{x0, x1}, DiscreteKeys{m1}, std::vector{c0, c1}); // Discrete uniform prior. hbn.emplace_shared(m1, "0.5/0.5"); diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 8b15b50d2..d0a4a79af 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -126,31 +126,34 @@ TEST(HybridGaussianElimination, IncrementalInference) { /********************************************************/ // Run batch elimination so we can compare results. - const Ordering ordering {X(0), X(1), X(2)}; + const Ordering ordering{X(0), X(1), X(2)}; // Now we calculate the expected factors using full elimination const auto [expectedHybridBayesTree, expectedRemainingGraph] = switching.linearizedFactorGraph.eliminatePartialMultifrontal(ordering); // The densities on X(0) should be the same - auto x0_conditional = - dynamic_pointer_cast(isam[X(0)]->conditional()->inner()); - auto expected_x0_conditional = dynamic_pointer_cast( - (*expectedHybridBayesTree)[X(0)]->conditional()->inner()); + auto x0_conditional = dynamic_pointer_cast( + isam[X(0)]->conditional()->inner()); + auto expected_x0_conditional = + dynamic_pointer_cast( + (*expectedHybridBayesTree)[X(0)]->conditional()->inner()); EXPECT(assert_equal(*x0_conditional, *expected_x0_conditional)); // The densities on X(1) should be the same - auto x1_conditional = - dynamic_pointer_cast(isam[X(1)]->conditional()->inner()); - auto expected_x1_conditional = dynamic_pointer_cast( - (*expectedHybridBayesTree)[X(1)]->conditional()->inner()); + auto x1_conditional = dynamic_pointer_cast( + isam[X(1)]->conditional()->inner()); + auto expected_x1_conditional = + dynamic_pointer_cast( + (*expectedHybridBayesTree)[X(1)]->conditional()->inner()); EXPECT(assert_equal(*x1_conditional, *expected_x1_conditional)); // The densities on X(2) should be the same - auto x2_conditional = - dynamic_pointer_cast(isam[X(2)]->conditional()->inner()); - auto expected_x2_conditional = dynamic_pointer_cast( - (*expectedHybridBayesTree)[X(2)]->conditional()->inner()); + auto x2_conditional = dynamic_pointer_cast( + isam[X(2)]->conditional()->inner()); + auto expected_x2_conditional = + dynamic_pointer_cast( + (*expectedHybridBayesTree)[X(2)]->conditional()->inner()); EXPECT(assert_equal(*x2_conditional, *expected_x2_conditional)); // We only perform manual continuous elimination for 0,0. @@ -381,11 +384,11 @@ TEST(HybridGaussianISAM, NonTrivial) { // Add connecting poses similar to PoseFactors in GTD fg.emplace_shared>(X(0), Y(0), Pose2(0, 1.0, 0), - poseNoise); + poseNoise); fg.emplace_shared>(Y(0), Z(0), Pose2(0, 1.0, 0), - poseNoise); + poseNoise); fg.emplace_shared>(Z(0), W(0), Pose2(0, 1.0, 0), - poseNoise); + poseNoise); // Create initial estimate Values initial; @@ -414,9 +417,9 @@ TEST(HybridGaussianISAM, NonTrivial) { KeyVector contKeys = {W(0), W(1)}; auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0); auto still = std::make_shared(W(0), W(1), Pose2(0, 0, 0), - noise_model), + noise_model), moving = std::make_shared(W(0), W(1), odometry, - noise_model); + noise_model); std::vector components = {moving, still}; auto mixtureFactor = std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); @@ -424,14 +427,14 @@ TEST(HybridGaussianISAM, NonTrivial) { // Add equivalent of ImuFactor fg.emplace_shared>(X(0), X(1), Pose2(1.0, 0.0, 0), - poseNoise); + poseNoise); // PoseFactors-like at k=1 fg.emplace_shared>(X(1), Y(1), Pose2(0, 1, 0), - poseNoise); + poseNoise); fg.emplace_shared>(Y(1), Z(1), Pose2(0, 1, 0), - poseNoise); + poseNoise); fg.emplace_shared>(Z(1), W(1), Pose2(-1, 1, 0), - poseNoise); + poseNoise); initial.insert(X(1), Pose2(1.0, 0.0, 0.0)); initial.insert(Y(1), Pose2(1.0, 1.0, 0.0)); @@ -454,7 +457,7 @@ TEST(HybridGaussianISAM, NonTrivial) { // Add odometry factor with discrete modes. contKeys = {W(1), W(2)}; still = std::make_shared(W(1), W(2), Pose2(0, 0, 0), - noise_model); + noise_model); moving = std::make_shared(W(1), W(2), odometry, noise_model); components = {moving, still}; @@ -464,14 +467,14 @@ TEST(HybridGaussianISAM, NonTrivial) { // Add equivalent of ImuFactor fg.emplace_shared>(X(1), X(2), Pose2(1.0, 0.0, 0), - poseNoise); + poseNoise); // PoseFactors-like at k=1 fg.emplace_shared>(X(2), Y(2), Pose2(0, 1, 0), - poseNoise); + poseNoise); fg.emplace_shared>(Y(2), Z(2), Pose2(0, 1, 0), - poseNoise); + poseNoise); fg.emplace_shared>(Z(2), W(2), Pose2(-2, 1, 0), - poseNoise); + poseNoise); initial.insert(X(2), Pose2(2.0, 0.0, 0.0)); initial.insert(Y(2), Pose2(2.0, 1.0, 0.0)); @@ -497,7 +500,7 @@ TEST(HybridGaussianISAM, NonTrivial) { // Add odometry factor with discrete modes. contKeys = {W(2), W(3)}; still = std::make_shared(W(2), W(3), Pose2(0, 0, 0), - noise_model); + noise_model); moving = std::make_shared(W(2), W(3), odometry, noise_model); components = {moving, still}; @@ -507,14 +510,14 @@ TEST(HybridGaussianISAM, NonTrivial) { // Add equivalent of ImuFactor fg.emplace_shared>(X(2), X(3), Pose2(1.0, 0.0, 0), - poseNoise); + poseNoise); // PoseFactors-like at k=3 fg.emplace_shared>(X(3), Y(3), Pose2(0, 1, 0), - poseNoise); + poseNoise); fg.emplace_shared>(Y(3), Z(3), Pose2(0, 1, 0), - poseNoise); + poseNoise); fg.emplace_shared>(Z(3), W(3), Pose2(-3, 1, 0), - poseNoise); + poseNoise); initial.insert(X(3), Pose2(3.0, 0.0, 0.0)); initial.insert(Y(3), Pose2(3.0, 1.0, 0.0)); diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp index 9e2794b19..71d5108a0 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp @@ -20,8 +20,8 @@ #include #include #include -#include #include +#include #include #include diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 3f9a18653..fd3494bea 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -23,8 +23,8 @@ #include #include #include -#include #include +#include #include #include #include @@ -150,8 +150,8 @@ TEST(HybridGaussianFactorGraph, Resize) { } /*************************************************************************** - * Test that the HybridNonlinearFactor reports correctly if the number of continuous - * keys provided do not match the keys in the factors. + * Test that the HybridNonlinearFactor reports correctly if the number of + * continuous keys provided do not match the keys in the factors. */ TEST(HybridGaussianFactorGraph, HybridNonlinearFactor) { auto nonlinearFactor = std::make_shared>( @@ -350,7 +350,8 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) { EliminateHybrid(factors, ordering); auto gaussianConditionalMixture = - dynamic_pointer_cast(hybridConditionalMixture->inner()); + dynamic_pointer_cast( + hybridConditionalMixture->inner()); CHECK(gaussianConditionalMixture); // Frontals = [x0, x1] @@ -413,7 +414,8 @@ TEST(HybridFactorGraph, PrintErrors) { // fg.print(); // std::cout << "\n\n\n" << std::endl; // fg.printErrors( - // HybridValues(hv.continuous(), DiscreteValues(), self.linearizationPoint)); + // HybridValues(hv.continuous(), DiscreteValues(), + // self.linearizationPoint)); } /**************************************************************************** diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index 06a4249d3..f4054c11a 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -143,7 +143,7 @@ TEST(HybridNonlinearISAM, IncrementalInference) { /********************************************************/ // Run batch elimination so we can compare results. - const Ordering ordering {X(0), X(1), X(2)}; + const Ordering ordering{X(0), X(1), X(2)}; // Now we calculate the actual factors using full elimination const auto [expectedHybridBayesTree, expectedRemainingGraph] = @@ -153,22 +153,25 @@ TEST(HybridNonlinearISAM, IncrementalInference) { // The densities on X(1) should be the same auto x0_conditional = dynamic_pointer_cast( bayesTree[X(0)]->conditional()->inner()); - auto expected_x0_conditional = dynamic_pointer_cast( - (*expectedHybridBayesTree)[X(0)]->conditional()->inner()); + auto expected_x0_conditional = + dynamic_pointer_cast( + (*expectedHybridBayesTree)[X(0)]->conditional()->inner()); EXPECT(assert_equal(*x0_conditional, *expected_x0_conditional)); // The densities on X(1) should be the same auto x1_conditional = dynamic_pointer_cast( bayesTree[X(1)]->conditional()->inner()); - auto expected_x1_conditional = dynamic_pointer_cast( - (*expectedHybridBayesTree)[X(1)]->conditional()->inner()); + auto expected_x1_conditional = + dynamic_pointer_cast( + (*expectedHybridBayesTree)[X(1)]->conditional()->inner()); EXPECT(assert_equal(*x1_conditional, *expected_x1_conditional)); // The densities on X(2) should be the same auto x2_conditional = dynamic_pointer_cast( bayesTree[X(2)]->conditional()->inner()); - auto expected_x2_conditional = dynamic_pointer_cast( - (*expectedHybridBayesTree)[X(2)]->conditional()->inner()); + auto expected_x2_conditional = + dynamic_pointer_cast( + (*expectedHybridBayesTree)[X(2)]->conditional()->inner()); EXPECT(assert_equal(*x2_conditional, *expected_x2_conditional)); // We only perform manual continuous elimination for 0,0. @@ -410,11 +413,11 @@ TEST(HybridNonlinearISAM, NonTrivial) { // Add connecting poses similar to PoseFactors in GTD fg.emplace_shared>(X(0), Y(0), Pose2(0, 1.0, 0), - poseNoise); + poseNoise); fg.emplace_shared>(Y(0), Z(0), Pose2(0, 1.0, 0), - poseNoise); + poseNoise); fg.emplace_shared>(Z(0), W(0), Pose2(0, 1.0, 0), - poseNoise); + poseNoise); // Create initial estimate Values initial; @@ -433,9 +436,9 @@ TEST(HybridNonlinearISAM, NonTrivial) { KeyVector contKeys = {W(0), W(1)}; auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0); auto still = std::make_shared(W(0), W(1), Pose2(0, 0, 0), - noise_model), + noise_model), moving = std::make_shared(W(0), W(1), odometry, - noise_model); + noise_model); std::vector components = {moving, still}; auto mixtureFactor = std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); @@ -443,14 +446,14 @@ TEST(HybridNonlinearISAM, NonTrivial) { // Add equivalent of ImuFactor fg.emplace_shared>(X(0), X(1), Pose2(1.0, 0.0, 0), - poseNoise); + poseNoise); // PoseFactors-like at k=1 fg.emplace_shared>(X(1), Y(1), Pose2(0, 1, 0), - poseNoise); + poseNoise); fg.emplace_shared>(Y(1), Z(1), Pose2(0, 1, 0), - poseNoise); + poseNoise); fg.emplace_shared>(Z(1), W(1), Pose2(-1, 1, 0), - poseNoise); + poseNoise); initial.insert(X(1), Pose2(1.0, 0.0, 0.0)); initial.insert(Y(1), Pose2(1.0, 1.0, 0.0)); @@ -473,7 +476,7 @@ TEST(HybridNonlinearISAM, NonTrivial) { // Add odometry factor with discrete modes. contKeys = {W(1), W(2)}; still = std::make_shared(W(1), W(2), Pose2(0, 0, 0), - noise_model); + noise_model); moving = std::make_shared(W(1), W(2), odometry, noise_model); components = {moving, still}; @@ -483,14 +486,14 @@ TEST(HybridNonlinearISAM, NonTrivial) { // Add equivalent of ImuFactor fg.emplace_shared>(X(1), X(2), Pose2(1.0, 0.0, 0), - poseNoise); + poseNoise); // PoseFactors-like at k=1 fg.emplace_shared>(X(2), Y(2), Pose2(0, 1, 0), - poseNoise); + poseNoise); fg.emplace_shared>(Y(2), Z(2), Pose2(0, 1, 0), - poseNoise); + poseNoise); fg.emplace_shared>(Z(2), W(2), Pose2(-2, 1, 0), - poseNoise); + poseNoise); initial.insert(X(2), Pose2(2.0, 0.0, 0.0)); initial.insert(Y(2), Pose2(2.0, 1.0, 0.0)); @@ -516,7 +519,7 @@ TEST(HybridNonlinearISAM, NonTrivial) { // Add odometry factor with discrete modes. contKeys = {W(2), W(3)}; still = std::make_shared(W(2), W(3), Pose2(0, 0, 0), - noise_model); + noise_model); moving = std::make_shared(W(2), W(3), odometry, noise_model); components = {moving, still}; @@ -526,14 +529,14 @@ TEST(HybridNonlinearISAM, NonTrivial) { // Add equivalent of ImuFactor fg.emplace_shared>(X(2), X(3), Pose2(1.0, 0.0, 0), - poseNoise); + poseNoise); // PoseFactors-like at k=3 fg.emplace_shared>(X(3), Y(3), Pose2(0, 1, 0), - poseNoise); + poseNoise); fg.emplace_shared>(Y(3), Z(3), Pose2(0, 1, 0), - poseNoise); + poseNoise); fg.emplace_shared>(Z(3), W(3), Pose2(-3, 1, 0), - poseNoise); + poseNoise); initial.insert(X(3), Pose2(3.0, 0.0, 0.0)); initial.insert(Y(3), Pose2(3.0, 1.0, 0.0)); diff --git a/gtsam/hybrid/tests/testSerializationHybrid.cpp b/gtsam/hybrid/tests/testSerializationHybrid.cpp index 76d919112..365dfcc9f 100644 --- a/gtsam/hybrid/tests/testSerializationHybrid.cpp +++ b/gtsam/hybrid/tests/testSerializationHybrid.cpp @@ -18,11 +18,11 @@ #include #include -#include -#include #include #include #include +#include +#include #include #include @@ -59,7 +59,8 @@ BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::Factors::Leaf, BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::Factors::Choice, "gtsam_HybridGaussianFactor_Factors_Choice"); -BOOST_CLASS_EXPORT_GUID(HybridGaussianConditional, "gtsam_HybridGaussianConditional"); +BOOST_CLASS_EXPORT_GUID(HybridGaussianConditional, + "gtsam_HybridGaussianConditional"); BOOST_CLASS_EXPORT_GUID(HybridGaussianConditional::Conditionals, "gtsam_HybridGaussianConditional_Conditionals"); BOOST_CLASS_EXPORT_GUID(HybridGaussianConditional::Conditionals::Leaf, @@ -115,7 +116,7 @@ TEST(HybridSerialization, HybridGaussianConditional) { const auto conditional1 = std::make_shared( GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 3)); const HybridGaussianConditional gm({Z(0)}, {X(0)}, {mode}, - {conditional0, conditional1}); + {conditional0, conditional1}); EXPECT(equalsObj(gm)); EXPECT(equalsXML(gm)); diff --git a/python/gtsam/tests/test_HybridBayesNet.py b/python/gtsam/tests/test_HybridBayesNet.py index 0fd569908..6e50c2bfe 100644 --- a/python/gtsam/tests/test_HybridBayesNet.py +++ b/python/gtsam/tests/test_HybridBayesNet.py @@ -51,7 +51,7 @@ class TestHybridBayesNet(GtsamTestCase): bayesNet.push_back(conditional) bayesNet.push_back( HybridGaussianConditional([X(1)], [], discrete_keys, - [conditional0, conditional1])) + [conditional0, conditional1])) bayesNet.push_back(DiscreteConditional(Asia, "99/1")) # Create values at which to evaluate. diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py index c7c4c6b51..f82665c49 100644 --- a/python/gtsam/tests/test_HybridFactorGraph.py +++ b/python/gtsam/tests/test_HybridFactorGraph.py @@ -27,6 +27,7 @@ DEBUG_MARGINALS = False class TestHybridGaussianFactorGraph(GtsamTestCase): """Unit tests for HybridGaussianFactorGraph.""" + def test_create(self): """Test construction of hybrid factor graph.""" model = noiseModel.Unit.Create(3) @@ -106,8 +107,9 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): I_1x1, X(0), [0], sigma=3) - bayesNet.push_back(HybridGaussianConditional([Z(i)], [X(0)], keys, - [conditional0, conditional1])) + bayesNet.push_back( + HybridGaussianConditional([Z(i)], [X(0)], keys, + [conditional0, conditional1])) # Create prior on X(0). prior_on_x0 = GaussianConditional.FromMeanAndStddev( @@ -219,9 +221,9 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): # Check ratio between unnormalized posterior and factor graph is the same for all modes: for mode in [1, 0]: values.insert_or_assign(M(0), mode) - self.assertAlmostEqual(bayesNet.evaluate(values) / - np.exp(-fg.error(values)), - 0.6366197723675815) + self.assertAlmostEqual( + bayesNet.evaluate(values) / np.exp(-fg.error(values)), + 0.6366197723675815) self.assertAlmostEqual(bayesNet.error(values), fg.error(values)) # Test elimination. diff --git a/python/gtsam/tests/test_HybridNonlinearFactorGraph.py b/python/gtsam/tests/test_HybridNonlinearFactorGraph.py index 3c2a70f73..3a659c4e8 100644 --- a/python/gtsam/tests/test_HybridNonlinearFactorGraph.py +++ b/python/gtsam/tests/test_HybridNonlinearFactorGraph.py @@ -24,12 +24,14 @@ from gtsam import BetweenFactorPoint3, Point3, PriorFactorPoint3, noiseModel class TestHybridGaussianFactorGraph(GtsamTestCase): """Unit tests for HybridGaussianFactorGraph.""" + def test_nonlinear_hybrid(self): nlfg = gtsam.HybridNonlinearFactorGraph() dk = gtsam.DiscreteKeys() dk.push_back((10, 2)) - nlfg.push_back(BetweenFactorPoint3(1, 2, Point3( - 1, 2, 3), noiseModel.Diagonal.Variances([1, 1, 1]))) + nlfg.push_back( + BetweenFactorPoint3(1, 2, Point3(1, 2, 3), + noiseModel.Diagonal.Variances([1, 1, 1]))) nlfg.push_back( PriorFactorPoint3(2, Point3(1, 2, 3), noiseModel.Diagonal.Variances([0.5, 0.5, 0.5]))) diff --git a/python/gtsam/tests/test_HybridValues.py b/python/gtsam/tests/test_HybridValues.py index 8a54d518c..73c27a4cd 100644 --- a/python/gtsam/tests/test_HybridValues.py +++ b/python/gtsam/tests/test_HybridValues.py @@ -14,11 +14,12 @@ from __future__ import print_function import unittest -import gtsam import numpy as np from gtsam.symbol_shorthand import C, X from gtsam.utils.test_case import GtsamTestCase +import gtsam + class TestHybridValues(GtsamTestCase): """Unit tests for HybridValues."""