From f39f678c14a0855c96a1302d142a7ad1906556a4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2024 12:02:42 -0400 Subject: [PATCH] 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);