From 8e85b68863ffdb451d7a729ff4d1bd89d2f4af32 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2024 11:48:20 -0400 Subject: [PATCH 01/21] formatting fixes --- gtsam/hybrid/HybridGaussianConditional.cpp | 8 ++--- gtsam/hybrid/HybridGaussianConditional.h | 2 +- gtsam/hybrid/HybridGaussianFactor.cpp | 29 ++++++++--------- gtsam/hybrid/HybridGaussianFactor.h | 17 +++++----- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 38 ++++++++++++---------- gtsam/hybrid/HybridGaussianFactorGraph.h | 6 ++-- 6 files changed, 50 insertions(+), 50 deletions(-) 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. From f39f678c14a0855c96a1302d142a7ad1906556a4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2024 12:02:42 -0400 Subject: [PATCH 02/21] add type info --- gtsam/hybrid/HybridGaussianConditional.cpp | 10 ++++++---- gtsam/hybrid/HybridGaussianFactor.cpp | 16 +++++++++------- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 8 +++++--- 3 files changed, 20 insertions(+), 14 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index d6ba2dbb6..2a0b5a875 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -183,10 +183,12 @@ bool HybridGaussianConditional::equals(const HybridFactor &lf, // Check the base and the factors: return BaseFactor::equals(*e, tol) && - conditionals_.equals( - e->conditionals_, [tol](const auto &f1, const auto &f2) { - return (!f1 && !f2) || (f1 && f2 && f1->equals(*f2, tol)); - }); + conditionals_.equals(e->conditionals_, + [tol](const GaussianConditional::shared_ptr &f1, + const GaussianConditional::shared_ptr &f2) { + return (!f1 && !f2) || + (f1 && f2 && f1->equals(*f2, tol)); + }); } /* *******************************************************************************/ diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index 8d03f18d0..e24545293 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -51,7 +51,7 @@ struct HybridGaussianFactor::ConstructorHelper { // Build the FactorValuePairs DecisionTree pairs = FactorValuePairs( DecisionTree(discreteKeys, factors), - [](const auto& f) { + [](const sharedFactor& f) { return std::pair{f, f ? 0.0 : std::numeric_limits::infinity()}; }); @@ -63,7 +63,7 @@ struct HybridGaussianFactor::ConstructorHelper { const std::vector& factorPairs) : discreteKeys({discreteKey}) { // Extract continuous keys from the first non-null factor - for (const auto& pair : factorPairs) { + for (const GaussianFactorValuePair& pair : factorPairs) { if (pair.first && continuousKeys.empty()) { continuousKeys = pair.first->keys(); break; @@ -121,7 +121,8 @@ bool HybridGaussianFactor::equals(const HybridFactor& lf, double tol) const { if (factors_.empty() ^ e->factors_.empty()) return false; // Check the base and the factors: - auto compareFunc = [tol](const auto& pair1, const auto& pair2) { + auto compareFunc = [tol](const GaussianFactorValuePair& pair1, + const GaussianFactorValuePair& pair2) { auto f1 = pair1.first, f2 = pair2.first; bool match = (!f1 && !f2) || (f1 && f2 && f1->equals(*f2, tol)); return match && gtsam::equal(pair1.second, pair2.second, tol); @@ -140,7 +141,7 @@ void HybridGaussianFactor::print(const std::string& s, } else { factors_.print( "", [&](Key k) { return formatter(k); }, - [&](const auto& pair) -> std::string { + [&](const GaussianFactorValuePair& pair) -> std::string { RedirectCout rd; std::cout << ":\n"; if (pair.first) { @@ -168,7 +169,8 @@ HybridGaussianProductFactor HybridGaussianFactor::asProductFactor() const { // - Each leaf converted to a GaussianFactorGraph with just the factor and its // scalar. return {{factors_, - [](const auto& pair) -> std::pair { + [](const GaussianFactorValuePair& pair) + -> std::pair { return {GaussianFactorGraph{pair.first}, pair.second}; }}}; } @@ -177,7 +179,7 @@ HybridGaussianProductFactor HybridGaussianFactor::asProductFactor() const { AlgebraicDecisionTree HybridGaussianFactor::errorTree( const VectorValues& continuousValues) const { // functor to convert from sharedFactor to double error value. - auto errorFunc = [&continuousValues](const auto& pair) { + auto errorFunc = [&continuousValues](const GaussianFactorValuePair& pair) { return pair.first ? pair.first->error(continuousValues) + pair.second : std::numeric_limits::infinity(); }; @@ -188,7 +190,7 @@ AlgebraicDecisionTree HybridGaussianFactor::errorTree( /* *******************************************************************************/ double HybridGaussianFactor::error(const HybridValues& values) const { // Directly index to get the component, no need to build the whole tree. - const auto pair = factors_(values.discrete()); + const GaussianFactorValuePair pair = factors_(values.discrete()); return pair.first ? pair.first->error(values.continuous()) + pair.second : std::numeric_limits::infinity(); } diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 62fbb2703..167e53656 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -57,7 +57,8 @@ using std::dynamic_pointer_cast; using OrphanWrapper = BayesTreeOrphanWrapper; using Result = std::pair, GaussianFactor::shared_ptr>; -using ResultTree = DecisionTree>; +using ResultValuePair = std::pair; +using ResultTree = DecisionTree; static const VectorValues kEmpty; @@ -305,7 +306,7 @@ static std::shared_ptr createHybridGaussianFactor( const ResultTree &eliminationResults, const DiscreteKeys &discreteSeparator) { // Correct for the normalization constant used up by the conditional - auto correct = [&](const auto &pair) -> GaussianFactorValuePair { + auto correct = [&](const ResultValuePair &pair) -> GaussianFactorValuePair { const auto &[conditional, factor] = pair.first; const double scalar = pair.second; if (conditional && factor) { @@ -384,7 +385,8 @@ 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 ResultValuePair &pair) { return pair.first.first; }); auto hybridGaussian = std::make_shared( discreteSeparator, conditionals); From 7603cd4871b9fbd813bf0c626974b1f772bd3d02 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2024 12:03:01 -0400 Subject: [PATCH 03/21] missing include and formatting --- gtsam/hybrid/tests/testHybridBayesTree.cpp | 29 +++++++++------------- 1 file changed, 12 insertions(+), 17 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridBayesTree.cpp b/gtsam/hybrid/tests/testHybridBayesTree.cpp index 319c6d52b..6da991173 100644 --- a/gtsam/hybrid/tests/testHybridBayesTree.cpp +++ b/gtsam/hybrid/tests/testHybridBayesTree.cpp @@ -22,6 +22,8 @@ #include #include +#include + #include "Switching.h" // Include for test suite @@ -62,7 +64,8 @@ std::vector components(Key key) { } // namespace two /* ************************************************************************* */ -TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) { +TEST(HybridGaussianFactorGraph, + HybridGaussianFactorGraphEliminateFullMultifrontalSimple) { HybridGaussianFactorGraph hfg; hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); @@ -179,10 +182,8 @@ TEST(HybridGaussianFactorGraph, Switching) { std::vector naturalX(N); std::iota(naturalX.begin(), naturalX.end(), 1); std::vector ordX; - std::transform( - naturalX.begin(), naturalX.end(), std::back_inserter(ordX), [](int x) { - return X(x); - }); + std::transform(naturalX.begin(), naturalX.end(), std::back_inserter(ordX), + [](int x) { return X(x); }); auto [ndX, lvls] = makeBinaryOrdering(ordX); std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering)); @@ -195,10 +196,8 @@ TEST(HybridGaussianFactorGraph, Switching) { std::vector naturalC(N - 1); std::iota(naturalC.begin(), naturalC.end(), 1); std::vector ordC; - std::transform( - naturalC.begin(), naturalC.end(), std::back_inserter(ordC), [](int x) { - return M(x); - }); + std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC), + [](int x) { return M(x); }); // std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering)); const auto [ndC, lvls] = makeBinaryOrdering(ordC); @@ -237,10 +236,8 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) { std::vector naturalX(N); std::iota(naturalX.begin(), naturalX.end(), 1); std::vector ordX; - std::transform( - naturalX.begin(), naturalX.end(), std::back_inserter(ordX), [](int x) { - return X(x); - }); + std::transform(naturalX.begin(), naturalX.end(), std::back_inserter(ordX), + [](int x) { return X(x); }); auto [ndX, lvls] = makeBinaryOrdering(ordX); std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering)); @@ -253,10 +250,8 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) { std::vector naturalC(N - 1); std::iota(naturalC.begin(), naturalC.end(), 1); std::vector ordC; - std::transform( - naturalC.begin(), naturalC.end(), std::back_inserter(ordC), [](int x) { - return M(x); - }); + std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC), + [](int x) { return M(x); }); // std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering)); const auto [ndC, lvls] = makeBinaryOrdering(ordC); From 874ba67693928a91b81597a8bf21872bcc118110 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2024 12:03:16 -0400 Subject: [PATCH 04/21] update comment --- gtsam/hybrid/tests/testHybridBayesNet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index d4192cb71..ad4e4e7a8 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -199,7 +199,7 @@ TEST(HybridBayesNet, Tiny) { factors_x0.push_back(fg.at(1)); auto productFactor = factors_x0.collectProductFactor(); - // Check that scalars are 0 and 1.79 + // Check that scalars are 0 and 1.79 (regression) EXPECT_DOUBLES_EQUAL(0.0, productFactor({{M(0), 0}}).second, 1e-9); EXPECT_DOUBLES_EQUAL(1.791759, productFactor({{M(0), 1}}).second, 1e-5); From 21b4c4c8d30ab75b4e8004c767b8cd411bbf35b2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2024 12:03:35 -0400 Subject: [PATCH 05/21] improve HybridGaussianProductFactor --- gtsam/hybrid/HybridGaussianProductFactor.cpp | 19 +++++++++++++++++-- gtsam/hybrid/HybridGaussianProductFactor.h | 7 +------ 2 files changed, 18 insertions(+), 8 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianProductFactor.cpp b/gtsam/hybrid/HybridGaussianProductFactor.cpp index 2e95ea8d1..375349f9b 100644 --- a/gtsam/hybrid/HybridGaussianProductFactor.cpp +++ b/gtsam/hybrid/HybridGaussianProductFactor.cpp @@ -26,39 +26,46 @@ namespace gtsam { using Y = HybridGaussianProductFactor::Y; +/* *******************************************************************************/ static Y add(const Y& y1, const Y& y2) { GaussianFactorGraph result = y1.first; result.push_back(y2.first); return {result, y1.second + y2.second}; }; +/* *******************************************************************************/ HybridGaussianProductFactor operator+(const HybridGaussianProductFactor& a, const HybridGaussianProductFactor& b) { return a.empty() ? b : HybridGaussianProductFactor(a.apply(b, add)); } +/* *******************************************************************************/ HybridGaussianProductFactor HybridGaussianProductFactor::operator+( const HybridGaussianFactor& factor) const { return *this + factor.asProductFactor(); } +/* *******************************************************************************/ HybridGaussianProductFactor HybridGaussianProductFactor::operator+( const GaussianFactor::shared_ptr& factor) const { return *this + HybridGaussianProductFactor(factor); } +/* *******************************************************************************/ HybridGaussianProductFactor& HybridGaussianProductFactor::operator+=( const GaussianFactor::shared_ptr& factor) { *this = *this + factor; return *this; } +/* *******************************************************************************/ HybridGaussianProductFactor& HybridGaussianProductFactor::operator+=( const HybridGaussianFactor& factor) { *this = *this + factor; return *this; } +/* *******************************************************************************/ void HybridGaussianProductFactor::print(const std::string& s, const KeyFormatter& formatter) const { KeySet keys; @@ -76,11 +83,19 @@ void HybridGaussianProductFactor::print(const std::string& s, } } +/* *******************************************************************************/ +bool HybridGaussianProductFactor::equals( + const HybridGaussianProductFactor& other, double tol) const { + return Base::equals(other, [tol](const Y& a, const Y& b) { + return a.first.equals(b.first, tol) && std::abs(a.second - b.second) < tol; + }); +} + +/* *******************************************************************************/ HybridGaussianProductFactor HybridGaussianProductFactor::removeEmpty() const { auto emptyGaussian = [](const Y& y) { bool hasNull = - std::any_of(y.first.begin(), - y.first.end(), + std::any_of(y.first.begin(), y.first.end(), [](const GaussianFactor::shared_ptr& ptr) { return !ptr; }); return hasNull ? Y{GaussianFactorGraph(), 0.0} : y; }; diff --git a/gtsam/hybrid/HybridGaussianProductFactor.h b/gtsam/hybrid/HybridGaussianProductFactor.h index 9c2aee74a..3658a0991 100644 --- a/gtsam/hybrid/HybridGaussianProductFactor.h +++ b/gtsam/hybrid/HybridGaussianProductFactor.h @@ -94,12 +94,7 @@ class HybridGaussianProductFactor * @return true if equal, false otherwise */ bool equals(const HybridGaussianProductFactor& other, - double tol = 1e-9) const { - return Base::equals(other, [tol](const Y& a, const Y& b) { - return a.first.equals(b.first, tol) && - std::abs(a.second - b.second) < tol; - }); - } + double tol = 1e-9) const; /// @} From 8b8466e04632dc62f49573608b21f2b8974c35f2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2024 15:05:05 -0400 Subject: [PATCH 06/21] formatting testHybridGaussianFactorGraph --- .../tests/testHybridGaussianFactorGraph.cpp | 39 +++++++++---------- 1 file changed, 18 insertions(+), 21 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 9fdc1aaea..980ae0bab 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -17,6 +17,8 @@ * @author Frank Dellaert */ +#include +#include #include #include #include @@ -37,9 +39,6 @@ #include #include -#include -#include - #include #include #include @@ -73,8 +72,8 @@ TEST(HybridGaussianFactorGraph, Creation) { HybridGaussianConditional gm( m0, {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)}); + std::make_shared(X(0), Vector3::Ones(), I_3x3, X(1), + I_3x3)}); hfg.add(gm); EXPECT_LONGS_EQUAL(2, hfg.size()); @@ -118,8 +117,8 @@ TEST(HybridGaussianFactorGraph, hybridEliminationOneFactor) { auto factor = std::dynamic_pointer_cast(result.second); CHECK(factor); // regression test - EXPECT( - assert_equal(DecisionTreeFactor{m1, "15.74961 15.74961"}, *factor, 1e-5)); + // Originally 15.74961, which is normalized to 1 + EXPECT(assert_equal(DecisionTreeFactor{m1, "1 1"}, *factor, 1e-5)); } /* ************************************************************************* */ @@ -177,7 +176,7 @@ TEST(HybridBayesNet, Switching) { Switching s(2, betweenSigma, priorSigma); // Check size of linearized factor graph - const HybridGaussianFactorGraph& graph = s.linearizedFactorGraph; + const HybridGaussianFactorGraph &graph = s.linearizedFactorGraph; EXPECT_LONGS_EQUAL(4, graph.size()); // Create some continuous and discrete values @@ -203,20 +202,20 @@ TEST(HybridBayesNet, Switching) { // Check error for M(0) = 0 const HybridValues values0{continuousValues, modeZero}; double expectedError0 = 0; - for (const auto& factor : graph) expectedError0 += factor->error(values0); + for (const auto &factor : graph) expectedError0 += factor->error(values0); EXPECT_DOUBLES_EQUAL(expectedError0, graph.error(values0), 1e-5); // Check error for M(0) = 1 const HybridValues values1{continuousValues, modeOne}; double expectedError1 = 0; - for (const auto& factor : graph) expectedError1 += factor->error(values1); + for (const auto &factor : graph) expectedError1 += factor->error(values1); EXPECT_DOUBLES_EQUAL(expectedError1, graph.error(values1), 1e-5); // Check errorTree AlgebraicDecisionTree actualErrors = graph.errorTree(continuousValues); // Create expected error tree - const AlgebraicDecisionTree expectedErrors( - M(0), expectedError0, expectedError1); + const AlgebraicDecisionTree expectedErrors(M(0), expectedError0, + expectedError1); // Check that the actual error tree matches the expected one EXPECT(assert_equal(expectedErrors, actualErrors, 1e-5)); @@ -232,8 +231,8 @@ TEST(HybridBayesNet, Switching) { const AlgebraicDecisionTree graphPosterior = graph.discretePosterior(continuousValues); const double sum = probPrime0 + probPrime1; - const AlgebraicDecisionTree expectedPosterior( - M(0), probPrime0 / sum, probPrime1 / sum); + const AlgebraicDecisionTree expectedPosterior(M(0), probPrime0 / sum, + probPrime1 / sum); EXPECT(assert_equal(expectedPosterior, graphPosterior, 1e-5)); // Make the clique of factors connected to x0: @@ -275,15 +274,13 @@ TEST(HybridBayesNet, Switching) { // Check that the scalars incorporate the negative log constant of the // conditional EXPECT_DOUBLES_EQUAL(scalar0 - (*p_x0_given_x1_m)(modeZero)->negLogConstant(), - (*phi_x1_m)(modeZero).second, - 1e-9); + (*phi_x1_m)(modeZero).second, 1e-9); EXPECT_DOUBLES_EQUAL(scalar1 - (*p_x0_given_x1_m)(modeOne)->negLogConstant(), - (*phi_x1_m)(modeOne).second, - 1e-9); + (*phi_x1_m)(modeOne).second, 1e-9); // Check that the conditional and remaining factor are consistent for both // modes - for (auto&& mode : {modeZero, modeOne}) { + for (auto &&mode : {modeZero, modeOne}) { const auto gc = (*p_x0_given_x1_m)(mode); const auto [gf, scalar] = (*phi_x1_m)(mode); @@ -342,7 +339,7 @@ TEST(HybridBayesNet, Switching) { // However, we can still check the total error for the clique factors_x1 and // the elimination results are equal, modulo -again- the negative log constant // of the conditional. - for (auto&& mode : {modeZero, modeOne}) { + for (auto &&mode : {modeZero, modeOne}) { auto gc_x1 = (*p_x1_given_m)(mode); double originalError_x1 = factors_x1.error({continuousValues, mode}); const double actualError = gc_x1->negLogConstant() + @@ -372,7 +369,7 @@ TEST(HybridGaussianFactorGraph, ErrorAndProbPrime) { Switching s(3); // Check size of linearized factor graph - const HybridGaussianFactorGraph& graph = s.linearizedFactorGraph; + const HybridGaussianFactorGraph &graph = s.linearizedFactorGraph; EXPECT_LONGS_EQUAL(7, graph.size()); // Eliminate the graph From 02d5421de2721e8ea8daf6aca45c8f955706aaca Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2024 15:12:04 -0400 Subject: [PATCH 07/21] add GTSAM_EXPORT to HybridGaussianProductFactor --- gtsam/hybrid/HybridGaussianProductFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridGaussianProductFactor.h b/gtsam/hybrid/HybridGaussianProductFactor.h index 3658a0991..b4c51dbd3 100644 --- a/gtsam/hybrid/HybridGaussianProductFactor.h +++ b/gtsam/hybrid/HybridGaussianProductFactor.h @@ -27,7 +27,7 @@ namespace gtsam { class HybridGaussianFactor; /// Alias for DecisionTree of GaussianFactorGraphs and their scalar sums -class HybridGaussianProductFactor +class GTSAM_EXPORT HybridGaussianProductFactor : public DecisionTree> { public: using Y = std::pair; From 711a07c01b71599f19213aeb9e4c0d79546b1848 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2024 16:49:07 -0400 Subject: [PATCH 08/21] small fix --- gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 980ae0bab..1aa4c8d49 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -117,8 +117,7 @@ TEST(HybridGaussianFactorGraph, hybridEliminationOneFactor) { auto factor = std::dynamic_pointer_cast(result.second); CHECK(factor); // regression test - // Originally 15.74961, which is normalized to 1 - EXPECT(assert_equal(DecisionTreeFactor{m1, "1 1"}, *factor, 1e-5)); + EXPECT(assert_equal(DecisionTreeFactor{m1, "15.74961 15.74961"}, *factor, 1e-5)); } /* ************************************************************************* */ From d58bd6cbafa20bb862fa264432379c1742cf914f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2024 18:28:58 -0400 Subject: [PATCH 09/21] get HybridBayesNet compiling on Windows --- gtsam/hybrid/HybridBayesNet.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index bba301be2..f46ce5249 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -25,6 +25,8 @@ #include #include +#include + namespace gtsam { /** From 0dbf13f9621d6f83112ddf68f3fd8e40f191038e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2024 18:41:52 -0400 Subject: [PATCH 10/21] Another Windows fix --- gtsam/hybrid/HybridConditional.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index 588c44e0b..3921df170 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -25,6 +25,7 @@ #include #include +#include #include #include #include From c60d3420a3561490db2f44b497dc840c3db385c5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2024 18:57:32 -0400 Subject: [PATCH 11/21] include in HybridConditional.cpp --- gtsam/hybrid/HybridConditional.cpp | 3 +++ gtsam/hybrid/HybridConditional.h | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index 175aec30c..463f3e283 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -13,6 +13,7 @@ * @file HybridConditional.cpp * @date Mar 11, 2022 * @author Fan Jiang + * @author Varun Agrawal */ #include @@ -21,6 +22,8 @@ #include #include +#include + namespace gtsam { /* ************************************************************************ */ diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index 3921df170..f3df725ef 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -13,6 +13,7 @@ * @file HybridConditional.h * @date Mar 11, 2022 * @author Fan Jiang + * @author Varun Agrawal */ #pragma once @@ -25,7 +26,6 @@ #include #include -#include #include #include #include From c08d6bdbed6bcf032b08215a3ffe15492cfe0bc9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2024 19:07:15 -0400 Subject: [PATCH 12/21] maybe it's --- gtsam/hybrid/HybridConditional.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index 463f3e283..f57237e93 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -22,7 +22,7 @@ #include #include -#include +#include namespace gtsam { From 6da1b019137e3634da33154abcf287ccb3e54961 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2024 19:25:38 -0400 Subject: [PATCH 13/21] include in HybridBayesTree.cpp --- gtsam/hybrid/HybridBayesTree.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index 0766f452b..df4a49f60 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -27,6 +27,7 @@ #include #include +#include #include "gtsam/hybrid/HybridConditional.h" From 03c467fe112befd8465d4f696d65fb9e35c2f2c0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2024 19:43:20 -0400 Subject: [PATCH 14/21] add more includes to try and debug this --- gtsam/hybrid/HybridConditional.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index f57237e93..2228b8cdc 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -22,6 +22,8 @@ #include #include +#include +#include #include namespace gtsam { From 8e29d57251aa0414030d0c2a19e18d03beaf3901 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2024 22:33:56 -0400 Subject: [PATCH 15/21] remove includes --- gtsam/hybrid/HybridBayesNet.h | 2 -- gtsam/hybrid/HybridBayesTree.cpp | 4 +--- gtsam/hybrid/HybridConditional.cpp | 3 --- 3 files changed, 1 insertion(+), 8 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index f46ce5249..bba301be2 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -25,8 +25,6 @@ #include #include -#include - namespace gtsam { /** diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index df4a49f60..193635a21 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -22,14 +22,12 @@ #include #include #include +#include #include #include #include #include -#include - -#include "gtsam/hybrid/HybridConditional.h" namespace gtsam { diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index 2228b8cdc..94d180727 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -22,9 +22,6 @@ #include #include -#include -#include -#include namespace gtsam { From 498079777d200d34bde12d1471593174ddc1a107 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2024 23:19:20 -0400 Subject: [PATCH 16/21] improved type aliasing --- gtsam/hybrid/HybridGaussianProductFactor.cpp | 2 +- gtsam/hybrid/HybridGaussianProductFactor.h | 9 +++++---- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianProductFactor.cpp b/gtsam/hybrid/HybridGaussianProductFactor.cpp index 375349f9b..e71201ceb 100644 --- a/gtsam/hybrid/HybridGaussianProductFactor.cpp +++ b/gtsam/hybrid/HybridGaussianProductFactor.cpp @@ -24,7 +24,7 @@ namespace gtsam { -using Y = HybridGaussianProductFactor::Y; +using Y = GaussianFactorGraphValuePair; /* *******************************************************************************/ static Y add(const Y& y1, const Y& y2) { diff --git a/gtsam/hybrid/HybridGaussianProductFactor.h b/gtsam/hybrid/HybridGaussianProductFactor.h index b4c51dbd3..508113e87 100644 --- a/gtsam/hybrid/HybridGaussianProductFactor.h +++ b/gtsam/hybrid/HybridGaussianProductFactor.h @@ -26,12 +26,13 @@ namespace gtsam { class HybridGaussianFactor; +using GaussianFactorGraphValuePair = std::pair; + /// Alias for DecisionTree of GaussianFactorGraphs and their scalar sums class GTSAM_EXPORT HybridGaussianProductFactor - : public DecisionTree> { + : public DecisionTree { public: - using Y = std::pair; - using Base = DecisionTree; + using Base = DecisionTree; /// @name Constructors /// @{ @@ -46,7 +47,7 @@ class GTSAM_EXPORT HybridGaussianProductFactor */ template HybridGaussianProductFactor(const std::shared_ptr& factor) - : Base(Y{GaussianFactorGraph{factor}, 0.0}) {} + : Base(GaussianFactorGraphValuePair{GaussianFactorGraph{factor}, 0.0}) {} /** * @brief Construct from DecisionTree From cd3e0f37fba6c6fdd1133b4a0fc0c17074572d81 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2024 23:20:41 -0400 Subject: [PATCH 17/21] include sstream in HybridGaussianProductFactor --- gtsam/hybrid/HybridGaussianProductFactor.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/hybrid/HybridGaussianProductFactor.cpp b/gtsam/hybrid/HybridGaussianProductFactor.cpp index e71201ceb..fc61e1c54 100644 --- a/gtsam/hybrid/HybridGaussianProductFactor.cpp +++ b/gtsam/hybrid/HybridGaussianProductFactor.cpp @@ -22,6 +22,8 @@ #include #include +#include + namespace gtsam { using Y = GaussianFactorGraphValuePair; From 4ae5596200b48146a4158f7f85087eace0404d70 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2024 23:21:46 -0400 Subject: [PATCH 18/21] add back PotentiallyPrunedComponentError as an inline function --- gtsam/hybrid/HybridGaussianFactor.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index e24545293..b31fdca20 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -175,13 +175,19 @@ HybridGaussianProductFactor HybridGaussianFactor::asProductFactor() const { }}}; } +/* *******************************************************************************/ +inline static double PotentiallyPrunedComponentError( + const GaussianFactorValuePair& pair, const VectorValues& continuousValues) { + return pair.first ? pair.first->error(continuousValues) + pair.second + : std::numeric_limits::infinity(); +} + /* *******************************************************************************/ AlgebraicDecisionTree HybridGaussianFactor::errorTree( const VectorValues& continuousValues) const { // functor to convert from sharedFactor to double error value. auto errorFunc = [&continuousValues](const GaussianFactorValuePair& pair) { - return pair.first ? pair.first->error(continuousValues) + pair.second - : std::numeric_limits::infinity(); + return PotentiallyPrunedComponentError(pair, continuousValues); }; DecisionTree error_tree(factors_, errorFunc); return error_tree; @@ -191,8 +197,7 @@ AlgebraicDecisionTree HybridGaussianFactor::errorTree( double HybridGaussianFactor::error(const HybridValues& values) const { // Directly index to get the component, no need to build the whole tree. const GaussianFactorValuePair pair = factors_(values.discrete()); - return pair.first ? pair.first->error(values.continuous()) + pair.second - : std::numeric_limits::infinity(); + return PotentiallyPrunedComponentError(pair, values.continuous()); } } // namespace gtsam From 26c9dcba74b3f33706d82bd34ccb4c1d90109b4e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2024 23:22:47 -0400 Subject: [PATCH 19/21] formatting --- gtsam/hybrid/HybridConditional.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index 94d180727..97ec1a1f8 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -22,7 +22,6 @@ #include #include - namespace gtsam { /* ************************************************************************ */ From 4df266ad64575c77346d4d8cd7a2dff593d027c7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2024 23:31:15 -0400 Subject: [PATCH 20/21] replace sstream with string --- gtsam/hybrid/HybridGaussianProductFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridGaussianProductFactor.cpp b/gtsam/hybrid/HybridGaussianProductFactor.cpp index fc61e1c54..30e1c57af 100644 --- a/gtsam/hybrid/HybridGaussianProductFactor.cpp +++ b/gtsam/hybrid/HybridGaussianProductFactor.cpp @@ -22,7 +22,7 @@ #include #include -#include +#include namespace gtsam { From 436524a4df30187c505c2acbd7e7affc0479590b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2024 23:55:19 -0400 Subject: [PATCH 21/21] use cout instead of stringstream --- gtsam/hybrid/HybridGaussianProductFactor.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianProductFactor.cpp b/gtsam/hybrid/HybridGaussianProductFactor.cpp index 30e1c57af..f7b5994f0 100644 --- a/gtsam/hybrid/HybridGaussianProductFactor.cpp +++ b/gtsam/hybrid/HybridGaussianProductFactor.cpp @@ -78,10 +78,9 @@ void HybridGaussianProductFactor::print(const std::string& s, }; Base::print(s, formatter, printer); if (!keys.empty()) { - std::stringstream ss; - ss << s << " Keys:"; - for (auto&& key : keys) ss << " " << formatter(key); - std::cout << ss.str() << "." << std::endl; + std::cout << s << " Keys:"; + for (auto&& key : keys) std::cout << " " << formatter(key); + std::cout << "." << std::endl; } }