diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index 42e702155..093d50fbf 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -220,22 +220,19 @@ std::shared_ptr HybridGaussianConditional::likelihood( const DiscreteKeys discreteParentKeys = discreteKeys(); const KeyVector continuousParentKeys = continuousParents(); const HybridGaussianFactor::Factors likelihoods( - conditionals_, [&](const GaussianConditional::shared_ptr &conditional) { - const auto likelihood_m = conditional->likelihood(given); + conditionals_, + [&](const GaussianConditional::shared_ptr &conditional) + -> std::pair { + auto likelihood_m = conditional->likelihood(given); const double Cgm_Kgcm = logConstant_ - conditional->logNormalizationConstant(); if (Cgm_Kgcm == 0.0) { - return likelihood_m; + return {likelihood_m, 0.0}; } else { // Add a constant factor to the likelihood in case the noise models // are not all equal. - GaussianFactorGraph gfg; - gfg.push_back(likelihood_m); - Vector c(1); - c << std::sqrt(2.0 * Cgm_Kgcm); - auto constantFactor = std::make_shared(c); - gfg.push_back(constantFactor); - return std::make_shared(gfg); + double c = std::sqrt(2.0 * Cgm_Kgcm); + return {likelihood_m, c}; } }); return std::make_shared( diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index f7207bf81..6fb453e75 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -46,8 +46,10 @@ bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const { // Check the base and the factors: return Base::equals(*e, tol) && factors_.equals(e->factors_, - [tol](const sharedFactor &f1, const sharedFactor &f2) { - return f1->equals(*f2, tol); + [tol](const std::pair &f1, + const std::pair &f2) { + return f1.first->equals(*f2.first, tol) && + (f1.second == f2.second); }); } @@ -63,11 +65,13 @@ void HybridGaussianFactor::print(const std::string &s, } else { factors_.print( "", [&](Key k) { return formatter(k); }, - [&](const sharedFactor &gf) -> std::string { + [&](const std::pair &gfv) -> std::string { + auto [gf, val] = gfv; RedirectCout rd; std::cout << ":\n"; if (gf) { gf->print("", formatter); + std::cout << "value: " << val << std::endl; return rd.str(); } else { return "nullptr"; @@ -78,8 +82,8 @@ void HybridGaussianFactor::print(const std::string &s, } /* *******************************************************************************/ -HybridGaussianFactor::sharedFactor HybridGaussianFactor::operator()( - const DiscreteValues &assignment) const { +std::pair +HybridGaussianFactor::operator()(const DiscreteValues &assignment) const { return factors_(assignment); } @@ -99,7 +103,9 @@ GaussianFactorGraphTree HybridGaussianFactor::add( /* *******************************************************************************/ GaussianFactorGraphTree HybridGaussianFactor::asGaussianFactorGraphTree() const { - auto wrap = [](const sharedFactor &gf) { return GaussianFactorGraph{gf}; }; + auto wrap = [](const std::pair &gfv) { + return GaussianFactorGraph{gfv.first}; + }; return {factors_, wrap}; } @@ -107,17 +113,19 @@ GaussianFactorGraphTree HybridGaussianFactor::asGaussianFactorGraphTree() AlgebraicDecisionTree HybridGaussianFactor::errorTree( const VectorValues &continuousValues) const { // functor to convert from sharedFactor to double error value. - auto errorFunc = [&continuousValues](const sharedFactor &gf) { - return gf->error(continuousValues); - }; + auto errorFunc = + [&continuousValues](const std::pair &gfv) { + auto [gf, val] = gfv; + return gf->error(continuousValues) + val; + }; DecisionTree error_tree(factors_, errorFunc); return error_tree; } /* *******************************************************************************/ double HybridGaussianFactor::error(const HybridValues &values) const { - const sharedFactor gf = factors_(values.discrete()); - return gf->error(values.continuous()); + auto &&[gf, val] = factors_(values.discrete()); + return gf->error(values.continuous()) + val; } } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 650f7a5a7..30c35dd2b 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -53,7 +53,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { using sharedFactor = std::shared_ptr; /// typedef for Decision Tree of Gaussian factors and log-constant. - using Factors = DecisionTree; + using Factors = DecisionTree>; private: /// Decision tree of Gaussian factors indexed by discrete keys. @@ -80,8 +80,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { * @param continuousKeys A vector of keys representing continuous variables. * @param discreteKeys A vector of keys representing discrete variables and * their cardinalities. - * @param factors The decision tree of Gaussian factors stored - * as the mixture density. + * @param factors The decision tree of Gaussian factors and arbitrary scalars. */ HybridGaussianFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, @@ -93,11 +92,12 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { * * @param continuousKeys Vector of keys for continuous factors. * @param discreteKeys Vector of discrete keys. - * @param factors Vector of gaussian factor shared pointers. + * @param factors Vector of gaussian factor shared pointers + * and arbitrary scalars. */ - HybridGaussianFactor(const KeyVector &continuousKeys, - const DiscreteKeys &discreteKeys, - const std::vector &factors) + HybridGaussianFactor( + const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, + const std::vector> &factors) : HybridGaussianFactor(continuousKeys, discreteKeys, Factors(discreteKeys, factors)) {} @@ -114,8 +114,9 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { /// @name Standard API /// @{ - /// Get factor at a given discrete assignment. - sharedFactor operator()(const DiscreteValues &assignment) const; + /// Get the factor and scalar at a given discrete assignment. + std::pair operator()( + const DiscreteValues &assignment) const; /** * @brief Combine the Gaussian Factor Graphs in `sum` and `this` while diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 900ec90c7..776f559a4 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -92,13 +92,15 @@ void HybridGaussianFactorGraph::printErrors( // Clear the stringstream ss.str(std::string()); - if (auto gmf = std::dynamic_pointer_cast(factor)) { + if (auto hgf = std::dynamic_pointer_cast(factor)) { if (factor == nullptr) { std::cout << "nullptr" << "\n"; } else { - gmf->operator()(values.discrete())->print(ss.str(), keyFormatter); - std::cout << "error = " << gmf->error(values) << std::endl; + auto [factor, val] = hgf->operator()(values.discrete()); + factor->print(ss.str(), keyFormatter); + std::cout << "value: " << val << std::endl; + std::cout << "error = " << factor->error(values) << std::endl; } } else if (auto hc = std::dynamic_pointer_cast(factor)) { if (factor == nullptr) { @@ -262,9 +264,12 @@ discreteElimination(const HybridGaussianFactorGraph &factors, // Case where we have a HybridGaussianFactor with no continuous keys. // In this case, compute discrete probabilities. auto logProbability = - [&](const GaussianFactor::shared_ptr &factor) -> double { - if (!factor) return 0.0; - return -factor->error(VectorValues()); + [&](const std::pair &fv) + -> double { + auto [factor, val] = fv; + double v = 0.5 * val * val; + if (!factor) return -v; + return -(factor->error(VectorValues()) + v); }; AlgebraicDecisionTree logProbabilities = DecisionTree(gmf->factors(), logProbability); @@ -348,7 +353,8 @@ static std::shared_ptr createHybridGaussianFactor( const KeyVector &continuousSeparator, const DiscreteKeys &discreteSeparator) { // Correct for the normalization constant used up by the conditional - auto correct = [&](const Result &pair) -> GaussianFactor::shared_ptr { + auto correct = + [&](const Result &pair) -> std::pair { const auto &[conditional, factor] = pair; if (factor) { auto hf = std::dynamic_pointer_cast(factor); @@ -357,10 +363,10 @@ static std::shared_ptr createHybridGaussianFactor( // as per the Hessian definition hf->constantTerm() += 2.0 * conditional->logNormalizationConstant(); } - return factor; + return {factor, 0.0}; }; - DecisionTree newFactors(eliminationResults, - correct); + DecisionTree> newFactors( + eliminationResults, correct); return std::make_shared(continuousSeparator, discreteSeparator, newFactors); @@ -597,10 +603,10 @@ GaussianFactorGraph HybridGaussianFactorGraph::operator()( gfg.push_back(gf); } else if (auto gc = std::dynamic_pointer_cast(f)) { gfg.push_back(gf); - } else if (auto gmf = std::dynamic_pointer_cast(f)) { - gfg.push_back((*gmf)(assignment)); - } else if (auto gm = dynamic_pointer_cast(f)) { - gfg.push_back((*gm)(assignment)); + } else if (auto hgf = std::dynamic_pointer_cast(f)) { + gfg.push_back((*hgf)(assignment).first); + } else if (auto hgc = dynamic_pointer_cast(f)) { + gfg.push_back((*hgc)(assignment)); } else { continue; } diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h index 2c2810f52..6a6055bd8 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.h +++ b/gtsam/hybrid/HybridNonlinearFactor.h @@ -245,10 +245,11 @@ class HybridNonlinearFactor : public HybridFactor { const Values& continuousValues) const { // functional to linearize each factor in the decision tree auto linearizeDT = - [continuousValues](const std::pair& f) { - auto [factor, val] = f; - return {factor->linearize(continuousValues), val}; - }; + [continuousValues](const std::pair& f) + -> std::pair { + auto [factor, val] = f; + return {factor->linearize(continuousValues), val}; + }; DecisionTree> linearized_factors(factors_, linearizeDT); diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp index 0b40ecc72..7e3e2defa 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp @@ -71,8 +71,10 @@ TEST(HybridGaussianFactor, Sum) { auto f20 = std::make_shared(X(1), A1, X(3), A3, b); auto f21 = std::make_shared(X(1), A1, X(3), A3, b); auto f22 = std::make_shared(X(1), A1, X(3), A3, b); - std::vector factorsA{f10, f11}; - std::vector factorsB{f20, f21, f22}; + std::vector> factorsA{ + {f10, 0.0}, {f11, 0.0}}; + std::vector> factorsB{ + {f20, 0.0}, {f21, 0.0}, {f22, 0.0}}; // TODO(Frank): why specify keys at all? And: keys in factor should be *all* // keys, deviating from Kevin's scheme. Should we index DT on DiscreteKey? @@ -109,7 +111,8 @@ TEST(HybridGaussianFactor, Printing) { auto b = Matrix::Zero(2, 1); auto f10 = std::make_shared(X(1), A1, X(2), A2, b); auto f11 = std::make_shared(X(1), A1, X(2), A2, b); - std::vector factors{f10, f11}; + std::vector> factors{ + {f10, 0.0}, {f11, 0.0}}; HybridGaussianFactor mixtureFactor({X(1), X(2)}, {m1}, factors); @@ -128,6 +131,7 @@ Hybrid [x1 x2; 1]{ ] b = [ 0 0 ] No noise model +value: 0 1 Leaf : A[x1] = [ @@ -140,6 +144,7 @@ Hybrid [x1 x2; 1]{ ] b = [ 0 0 ] No noise model +value: 0 } )"; @@ -178,7 +183,8 @@ TEST(HybridGaussianFactor, Error) { auto f0 = std::make_shared(X(1), A01, X(2), A02, b); auto f1 = std::make_shared(X(1), A11, X(2), A12, b); - std::vector factors{f0, f1}; + std::vector> factors{{f0, 0.0}, + {f1, 0.0}}; HybridGaussianFactor mixtureFactor({X(1), X(2)}, {m1}, factors);