diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index 4dcb84034..d6ba2dbb6 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -79,7 +79,7 @@ struct HybridGaussianConditional::Helper { explicit Helper(const Conditionals &conditionals) : conditionals(conditionals), minNegLogConstant(std::numeric_limits::infinity()) { - auto func = [this](const GC::shared_ptr& gc) -> GaussianFactorValuePair { + auto func = [this](const GC::shared_ptr &gc) -> GaussianFactorValuePair { if (!gc) return {nullptr, std::numeric_limits::infinity()}; if (!nrFrontals) nrFrontals = gc->nrFrontals(); double value = gc->negLogConstant(); @@ -97,10 +97,10 @@ struct HybridGaussianConditional::Helper { /* *******************************************************************************/ HybridGaussianConditional::HybridGaussianConditional( - const DiscreteKeys& discreteParents, const Helper& helper) + const DiscreteKeys &discreteParents, const Helper &helper) : BaseFactor(discreteParents, FactorValuePairs(helper.pairs, - [&](const GaussianFactorValuePair& + [&](const GaussianFactorValuePair & pair) { // subtract minNegLogConstant return GaussianFactorValuePair{ pair.first, @@ -225,7 +225,7 @@ KeyVector HybridGaussianConditional::continuousParents() const { // remove that key from continuousParentKeys: continuousParentKeys.erase(std::remove(continuousParentKeys.begin(), continuousParentKeys.end(), key), - continuousParentKeys.end()); + continuousParentKeys.end()); } return continuousParentKeys; } diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index 25391cb83..75138b0dc 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -24,9 +24,9 @@ #include #include #include -#include #include #include +#include #include #include diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index afc818c4a..8d03f18d0 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -93,27 +93,27 @@ struct HybridGaussianFactor::ConstructorHelper { }; /* *******************************************************************************/ -HybridGaussianFactor::HybridGaussianFactor(const ConstructorHelper &helper) +HybridGaussianFactor::HybridGaussianFactor(const ConstructorHelper& helper) : Base(helper.continuousKeys, helper.discreteKeys), factors_(helper.pairs) {} HybridGaussianFactor::HybridGaussianFactor( - const DiscreteKey &discreteKey, - const std::vector &factors) + const DiscreteKey& discreteKey, + const std::vector& factors) : HybridGaussianFactor(ConstructorHelper(discreteKey, factors)) {} HybridGaussianFactor::HybridGaussianFactor( - const DiscreteKey &discreteKey, - const std::vector &factorPairs) + const DiscreteKey& discreteKey, + const std::vector& factorPairs) : HybridGaussianFactor(ConstructorHelper(discreteKey, factorPairs)) {} -HybridGaussianFactor::HybridGaussianFactor(const DiscreteKeys &discreteKeys, - const FactorValuePairs &factors) +HybridGaussianFactor::HybridGaussianFactor(const DiscreteKeys& discreteKeys, + const FactorValuePairs& factors) : HybridGaussianFactor(ConstructorHelper(discreteKeys, factors)) {} /* *******************************************************************************/ -bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const { - const This *e = dynamic_cast(&lf); +bool HybridGaussianFactor::equals(const HybridFactor& lf, double tol) const { + const This* e = dynamic_cast(&lf); if (e == nullptr) return false; // This will return false if either factors_ is empty or e->factors_ is @@ -130,8 +130,8 @@ bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const { } /* *******************************************************************************/ -void HybridGaussianFactor::print(const std::string &s, - const KeyFormatter &formatter) const { +void HybridGaussianFactor::print(const std::string& s, + const KeyFormatter& formatter) const { std::cout << (s.empty() ? "" : s + "\n"); HybridFactor::print("", formatter); std::cout << "{\n"; @@ -139,8 +139,7 @@ void HybridGaussianFactor::print(const std::string &s, std::cout << " empty" << std::endl; } else { factors_.print( - "", - [&](Key k) { return formatter(k); }, + "", [&](Key k) { return formatter(k); }, [&](const auto& pair) -> std::string { RedirectCout rd; std::cout << ":\n"; @@ -158,7 +157,7 @@ void HybridGaussianFactor::print(const std::string &s, /* *******************************************************************************/ GaussianFactorValuePair HybridGaussianFactor::operator()( - const DiscreteValues &assignment) const { + const DiscreteValues& assignment) const { return factors_(assignment); } @@ -176,7 +175,7 @@ HybridGaussianProductFactor HybridGaussianFactor::asProductFactor() const { /* *******************************************************************************/ AlgebraicDecisionTree HybridGaussianFactor::errorTree( - const VectorValues &continuousValues) const { + const VectorValues& continuousValues) const { // functor to convert from sharedFactor to double error value. auto errorFunc = [&continuousValues](const auto& pair) { return pair.first ? pair.first->error(continuousValues) + pair.second diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 48a8bf48d..6c06c1c0a 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -58,7 +58,7 @@ using GaussianFactorValuePair = std::pair; * @ingroup hybrid */ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { -public: + public: using Base = HybridFactor; using This = HybridGaussianFactor; using shared_ptr = std::shared_ptr; @@ -68,11 +68,11 @@ public: /// typedef for Decision Tree of Gaussian factors and arbitrary value. using FactorValuePairs = DecisionTree; -private: + private: /// Decision tree of Gaussian factors indexed by discrete keys. FactorValuePairs factors_; -public: + public: /// @name Constructors /// @{ @@ -120,9 +120,8 @@ public: bool equals(const HybridFactor &lf, double tol = 1e-9) const override; - void - print(const std::string &s = "", - const KeyFormatter &formatter = DefaultKeyFormatter) const override; + void print(const std::string &s = "", const KeyFormatter &formatter = + DefaultKeyFormatter) const override; /// @} /// @name Standard API @@ -138,8 +137,8 @@ public: * @return AlgebraicDecisionTree A decision tree with the same keys * as the factors involved, and leaf values as the error. */ - AlgebraicDecisionTree - errorTree(const VectorValues &continuousValues) const override; + AlgebraicDecisionTree errorTree( + const VectorValues &continuousValues) const override; /** * @brief Compute the log-likelihood, including the log-normalizing constant. @@ -159,7 +158,7 @@ public: /// @} -private: + private: /** * @brief Helper function to augment the [A|b] matrices in the factor * components with the additional scalar values. This is done by storing the diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 26cdb8dd2..62fbb2703 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -39,7 +40,6 @@ #include #include #include -#include "gtsam/discrete/DiscreteValues.h" #include #include @@ -63,9 +63,9 @@ static const VectorValues kEmpty; /* ************************************************************************ */ // Throw a runtime exception for method specified in string s, and factor f: -static void throwRuntimeError(const std::string& s, - const std::shared_ptr& f) { - auto& fr = *f; +static void throwRuntimeError(const std::string &s, + const std::shared_ptr &f) { + auto &fr = *f; throw std::runtime_error(s + " not implemented for factor type " + demangle(typeid(fr).name()) + "."); } @@ -83,11 +83,12 @@ static void printFactor(const std::shared_ptr &factor, const DiscreteValues &assignment, const KeyFormatter &keyFormatter) { if (auto hgf = dynamic_pointer_cast(factor)) { - if (assignment.empty()) + if (assignment.empty()) { hgf->print("HybridGaussianFactor:", keyFormatter); - else + } else { hgf->operator()(assignment) .first->print("HybridGaussianFactor, component:", keyFormatter); + } } else if (auto gf = dynamic_pointer_cast(factor)) { factor->print("GaussianFactor:\n", keyFormatter); @@ -99,12 +100,13 @@ static void printFactor(const std::shared_ptr &factor, } else if (hc->isDiscrete()) { factor->print("DiscreteConditional:\n", keyFormatter); } else { - if (assignment.empty()) + if (assignment.empty()) { hc->print("HybridConditional:", keyFormatter); - else + } else { hc->asHybrid() ->choose(assignment) ->print("HybridConditional, component:\n", keyFormatter); + } } } else { factor->print("Unknown factor type\n", keyFormatter); @@ -112,13 +114,13 @@ static void printFactor(const std::shared_ptr &factor, } /* ************************************************************************ */ -void HybridGaussianFactorGraph::print(const std::string& s, - const KeyFormatter& keyFormatter) const { +void HybridGaussianFactorGraph::print(const std::string &s, + const KeyFormatter &keyFormatter) const { std::cout << (s.empty() ? "" : s + " ") << std::endl; std::cout << "size: " << size() << std::endl; for (size_t i = 0; i < factors_.size(); i++) { - auto&& factor = factors_[i]; + auto &&factor = factors_[i]; if (factor == nullptr) { std::cout << "Factor " << i << ": nullptr\n"; continue; @@ -163,7 +165,7 @@ HybridGaussianProductFactor HybridGaussianFactorGraph::collectProductFactor() const { HybridGaussianProductFactor result; - for (auto& f : factors_) { + for (auto &f : factors_) { // TODO(dellaert): can we make this cleaner and less error-prone? if (auto orphan = dynamic_pointer_cast(f)) { continue; // Ignore OrphanWrapper @@ -235,7 +237,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors, } else if (auto gmf = dynamic_pointer_cast(f)) { // Case where we have a HybridGaussianFactor with no continuous keys. // In this case, compute discrete probabilities. - auto potential = [&](const auto& pair) -> double { + auto potential = [&](const auto &pair) -> double { auto [factor, scalar] = pair; // If factor is null, it has been pruned, hence return potential zero if (!factor) return 0.0; @@ -270,10 +272,10 @@ discreteElimination(const HybridGaussianFactorGraph &factors, * depends on the discrete separator if present. */ static std::shared_ptr createDiscreteFactor( - const ResultTree& eliminationResults, + const ResultTree &eliminationResults, const DiscreteKeys &discreteSeparator) { auto potential = [&](const auto &pair) -> double { - const auto& [conditional, factor] = pair.first; + const auto &[conditional, factor] = pair.first; const double scalar = pair.second; if (conditional && factor) { // `error` has the following contributions: @@ -350,9 +352,9 @@ HybridGaussianFactorGraph::eliminate(const Ordering &keys) const { // This is the elimination method on the leaf nodes bool someContinuousLeft = false; - auto eliminate = [&](const std::pair& pair) + auto eliminate = [&](const std::pair &pair) -> std::pair { - const auto& [graph, scalar] = pair; + const auto &[graph, scalar] = pair; if (graph.empty()) { return {{nullptr, nullptr}, 0.0}; @@ -382,7 +384,7 @@ HybridGaussianFactorGraph::eliminate(const Ordering &keys) const { // Create the HybridGaussianConditional from the conditionals HybridGaussianConditional::Conditionals conditionals( - eliminationResults, [](const auto& pair) { return pair.first.first; }); + eliminationResults, [](const auto &pair) { return pair.first.first; }); auto hybridGaussian = std::make_shared( discreteSeparator, conditionals); diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index d72e81489..c503118eb 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -145,9 +145,9 @@ class GTSAM_EXPORT HybridGaussianFactorGraph /// @name Testable /// @{ - void - print(const std::string &s = "HybridGaussianFactorGraph", - const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override; + void print( + const std::string& s = "HybridGaussianFactorGraph", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; /** * @brief Print the errors of each factor in the hybrid factor graph.