diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index fc91e08389..8d6fef3f43 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -32,9 +32,6 @@ namespace gtsam { class HybridValues; -/// Alias for DecisionTree of GaussianFactorGraphs -using GaussianFactorGraphTree = DecisionTree; - KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys); KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2); diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index 2c0fb28a40..4dcb840341 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -32,6 +33,16 @@ namespace gtsam { /* *******************************************************************************/ +/** + * @brief Helper struct for constructing HybridGaussianConditional objects + * + * This struct contains the following fields: + * - nrFrontals: Optional size_t for number of frontal variables + * - pairs: FactorValuePairs for storing conditionals with their negLogConstant + * - conditionals: Conditionals for storing conditionals. TODO(frank): kill! + * - minNegLogConstant: minimum negLogConstant, computed here, subtracted in + * constructor + */ struct HybridGaussianConditional::Helper { std::optional nrFrontals; FactorValuePairs pairs; @@ -68,16 +79,12 @@ struct HybridGaussianConditional::Helper { explicit Helper(const Conditionals &conditionals) : conditionals(conditionals), minNegLogConstant(std::numeric_limits::infinity()) { - auto func = [this](const GC::shared_ptr &c) -> GaussianFactorValuePair { - double value = 0.0; - if (c) { - if (!nrFrontals.has_value()) { - nrFrontals = c->nrFrontals(); - } - value = c->negLogConstant(); - minNegLogConstant = std::min(minNegLogConstant, value); - } - return {std::dynamic_pointer_cast(c), value}; + 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(); + minNegLogConstant = std::min(minNegLogConstant, value); + return {gc, value}; }; pairs = FactorValuePairs(conditionals, func); if (!nrFrontals.has_value()) { @@ -90,8 +97,15 @@ struct HybridGaussianConditional::Helper { /* *******************************************************************************/ HybridGaussianConditional::HybridGaussianConditional( - const DiscreteKeys &discreteParents, const Helper &helper) - : BaseFactor(discreteParents, helper.pairs), + const DiscreteKeys& discreteParents, const Helper& helper) + : BaseFactor(discreteParents, + FactorValuePairs(helper.pairs, + [&](const GaussianFactorValuePair& + pair) { // subtract minNegLogConstant + return GaussianFactorValuePair{ + pair.first, + pair.second - helper.minNegLogConstant}; + })), BaseConditional(*helper.nrFrontals), conditionals_(helper.conditionals), negLogConstant_(helper.minNegLogConstant) {} @@ -135,29 +149,6 @@ HybridGaussianConditional::conditionals() const { return conditionals_; } -/* *******************************************************************************/ -GaussianFactorGraphTree HybridGaussianConditional::asGaussianFactorGraphTree() - const { - auto wrap = [this](const GaussianConditional::shared_ptr &gc) { - // First check if conditional has not been pruned - if (gc) { - const double Cgm_Kgcm = gc->negLogConstant() - this->negLogConstant_; - // If there is a difference in the covariances, we need to account for - // that since the error is dependent on the mode. - if (Cgm_Kgcm > 0.0) { - // We add a constant factor which will be used when computing - // the probability of the discrete variables. - Vector c(1); - c << std::sqrt(2.0 * Cgm_Kgcm); - auto constantFactor = std::make_shared(c); - return GaussianFactorGraph{gc, constantFactor}; - } - } - return GaussianFactorGraph{gc}; - }; - return {conditionals_, wrap}; -} - /* *******************************************************************************/ size_t HybridGaussianConditional::nrComponents() const { size_t total = 0; @@ -202,9 +193,6 @@ bool HybridGaussianConditional::equals(const HybridFactor &lf, void HybridGaussianConditional::print(const std::string &s, const KeyFormatter &formatter) const { std::cout << (s.empty() ? "" : s + "\n"); - if (isContinuous()) std::cout << "Continuous "; - if (isDiscrete()) std::cout << "Discrete "; - if (isHybrid()) std::cout << "Hybrid "; BaseConditional::print("", formatter); std::cout << " Discrete Keys = "; for (auto &dk : discreteKeys()) { @@ -237,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; } @@ -270,13 +258,7 @@ std::shared_ptr HybridGaussianConditional::likelihood( -> GaussianFactorValuePair { const auto likelihood_m = conditional->likelihood(given); const double Cgm_Kgcm = conditional->negLogConstant() - negLogConstant_; - if (Cgm_Kgcm == 0.0) { - return {likelihood_m, 0.0}; - } else { - // Add a constant to the likelihood in case the noise models - // are not all equal. - return {likelihood_m, Cgm_Kgcm}; - } + return {likelihood_m, Cgm_Kgcm}; }); return std::make_shared(discreteParentKeys, likelihoods); diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index 27e31d7670..25391cb837 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -231,9 +232,6 @@ class GTSAM_EXPORT HybridGaussianConditional HybridGaussianConditional(const DiscreteKeys &discreteParents, const Helper &helper); - /// Convert to a DecisionTree of Gaussian factor graphs. - GaussianFactorGraphTree asGaussianFactorGraphTree() const; - /// Check whether `given` has values for all frontal keys. bool allFrontalsGiven(const VectorValues &given) const; diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index b04db4977e..afc818c4a7 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -18,83 +18,52 @@ * @date Mar 12, 2022 */ +#include #include #include #include #include #include +#include #include #include #include namespace gtsam { -/* *******************************************************************************/ -HybridGaussianFactor::Factors HybridGaussianFactor::augment( - const FactorValuePairs &factors) { - // Find the minimum value so we can "proselytize" to positive values. - // Done because we can't have sqrt of negative numbers. - Factors gaussianFactors; - AlgebraicDecisionTree valueTree; - std::tie(gaussianFactors, valueTree) = unzip(factors); - - // Compute minimum value for normalization. - double min_value = valueTree.min(); - - // Finally, update the [A|b] matrices. - auto update = [&min_value](const GaussianFactorValuePair &gfv) { - auto [gf, value] = gfv; - - auto jf = std::dynamic_pointer_cast(gf); - if (!jf) return gf; - - double normalized_value = value - min_value; - - // If the value is 0, do nothing - if (normalized_value == 0.0) return gf; - - GaussianFactorGraph gfg; - gfg.push_back(jf); - - Vector c(1); - // When hiding c inside the `b` vector, value == 0.5*c^2 - c << std::sqrt(2.0 * normalized_value); - auto constantFactor = std::make_shared(c); - - gfg.push_back(constantFactor); - return std::dynamic_pointer_cast( - std::make_shared(gfg)); - }; - return Factors(factors, update); -} - /* *******************************************************************************/ struct HybridGaussianFactor::ConstructorHelper { KeyVector continuousKeys; // Continuous keys extracted from factors DiscreteKeys discreteKeys; // Discrete keys provided to the constructors - FactorValuePairs pairs; // Used only if factorsTree is empty - Factors factorsTree; + FactorValuePairs pairs; // The decision tree with factors and scalars - ConstructorHelper(const DiscreteKey &discreteKey, - const std::vector &factors) + /// Constructor for a single discrete key and a vector of Gaussian factors + ConstructorHelper(const DiscreteKey& discreteKey, + const std::vector& factors) : discreteKeys({discreteKey}) { // Extract continuous keys from the first non-null factor - for (const auto &factor : factors) { + for (const auto& factor : factors) { if (factor && continuousKeys.empty()) { continuousKeys = factor->keys(); break; } } - - // Build the DecisionTree from the factor vector - factorsTree = Factors(discreteKeys, factors); + // Build the FactorValuePairs DecisionTree + pairs = FactorValuePairs( + DecisionTree(discreteKeys, factors), + [](const auto& f) { + return std::pair{f, + f ? 0.0 : std::numeric_limits::infinity()}; + }); } - ConstructorHelper(const DiscreteKey &discreteKey, - const std::vector &factorPairs) + /// Constructor for a single discrete key and a vector of + /// GaussianFactorValuePairs + ConstructorHelper(const DiscreteKey& discreteKey, + const std::vector& factorPairs) : discreteKeys({discreteKey}) { // Extract continuous keys from the first non-null factor - for (const auto &pair : factorPairs) { + for (const auto& pair : factorPairs) { if (pair.first && continuousKeys.empty()) { continuousKeys = pair.first->keys(); break; @@ -105,11 +74,14 @@ struct HybridGaussianFactor::ConstructorHelper { pairs = FactorValuePairs(discreteKeys, factorPairs); } - ConstructorHelper(const DiscreteKeys &discreteKeys, - const FactorValuePairs &factorPairs) + /// Constructor for a vector of discrete keys and a vector of + /// GaussianFactorValuePairs + ConstructorHelper(const DiscreteKeys& discreteKeys, + const FactorValuePairs& factorPairs) : discreteKeys(discreteKeys) { // Extract continuous keys from the first non-null factor - factorPairs.visit([&](const GaussianFactorValuePair &pair) { + // TODO: just stop after first non-null factor + factorPairs.visit([&](const GaussianFactorValuePair& pair) { if (pair.first && continuousKeys.empty()) { continuousKeys = pair.first->keys(); } @@ -123,22 +95,18 @@ struct HybridGaussianFactor::ConstructorHelper { /* *******************************************************************************/ HybridGaussianFactor::HybridGaussianFactor(const ConstructorHelper &helper) : Base(helper.continuousKeys, helper.discreteKeys), - factors_(helper.factorsTree.empty() ? augment(helper.pairs) - : helper.factorsTree) {} + factors_(helper.pairs) {} -/* *******************************************************************************/ HybridGaussianFactor::HybridGaussianFactor( const DiscreteKey &discreteKey, const std::vector &factors) : HybridGaussianFactor(ConstructorHelper(discreteKey, factors)) {} -/* *******************************************************************************/ HybridGaussianFactor::HybridGaussianFactor( const DiscreteKey &discreteKey, const std::vector &factorPairs) : HybridGaussianFactor(ConstructorHelper(discreteKey, factorPairs)) {} -/* *******************************************************************************/ HybridGaussianFactor::HybridGaussianFactor(const DiscreteKeys &discreteKeys, const FactorValuePairs &factors) : HybridGaussianFactor(ConstructorHelper(discreteKeys, factors)) {} @@ -153,29 +121,32 @@ bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const { if (factors_.empty() ^ e->factors_.empty()) return false; // Check the base and the factors: - return Base::equals(*e, tol) && - factors_.equals(e->factors_, [tol](const auto &f1, const auto &f2) { - return (!f1 && !f2) || (f1 && f2 && f1->equals(*f2, tol)); - }); + auto compareFunc = [tol](const auto& pair1, const auto& 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); + }; + return Base::equals(*e, tol) && factors_.equals(e->factors_, compareFunc); } /* *******************************************************************************/ void HybridGaussianFactor::print(const std::string &s, const KeyFormatter &formatter) const { std::cout << (s.empty() ? "" : s + "\n"); - std::cout << "HybridGaussianFactor" << std::endl; HybridFactor::print("", formatter); std::cout << "{\n"; if (factors_.empty()) { std::cout << " empty" << std::endl; } else { factors_.print( - "", [&](Key k) { return formatter(k); }, - [&](const sharedFactor &gf) -> std::string { + "", + [&](Key k) { return formatter(k); }, + [&](const auto& pair) -> std::string { RedirectCout rd; std::cout << ":\n"; - if (gf) { - gf->print("", formatter); + if (pair.first) { + pair.first->print("", formatter); + std::cout << "scalar: " << pair.second << "\n"; return rd.str(); } else { return "nullptr"; @@ -186,61 +157,41 @@ void HybridGaussianFactor::print(const std::string &s, } /* *******************************************************************************/ -HybridGaussianFactor::sharedFactor HybridGaussianFactor::operator()( +GaussianFactorValuePair HybridGaussianFactor::operator()( const DiscreteValues &assignment) const { return factors_(assignment); } /* *******************************************************************************/ -GaussianFactorGraphTree HybridGaussianFactor::add( - const GaussianFactorGraphTree &sum) const { - using Y = GaussianFactorGraph; - auto add = [](const Y &graph1, const Y &graph2) { - auto result = graph1; - result.push_back(graph2); - return result; - }; - const auto tree = asGaussianFactorGraphTree(); - return sum.empty() ? tree : sum.apply(tree, add); -} - -/* *******************************************************************************/ -GaussianFactorGraphTree HybridGaussianFactor::asGaussianFactorGraphTree() - const { - auto wrap = [](const sharedFactor &gf) { return GaussianFactorGraph{gf}; }; - return {factors_, wrap}; -} - -/* *******************************************************************************/ -/// Helper method to compute the error of a component. -static double PotentiallyPrunedComponentError( - const GaussianFactor::shared_ptr &gf, const VectorValues &values) { - // Check if valid pointer - if (gf) { - return gf->error(values); - } else { - // If nullptr this component was pruned, so we return maximum error. This - // way the negative exponential will give a probability value close to 0.0. - return std::numeric_limits::max(); - } +HybridGaussianProductFactor HybridGaussianFactor::asProductFactor() const { + // Implemented by creating a new DecisionTree where: + // - The structure (keys and assignments) is preserved from factors_ + // - Each leaf converted to a GaussianFactorGraph with just the factor and its + // scalar. + return {{factors_, + [](const auto& pair) -> std::pair { + return {GaussianFactorGraph{pair.first}, pair.second}; + }}}; } /* *******************************************************************************/ AlgebraicDecisionTree HybridGaussianFactor::errorTree( const VectorValues &continuousValues) const { // functor to convert from sharedFactor to double error value. - auto errorFunc = [&continuousValues](const sharedFactor &gf) { - return PotentiallyPrunedComponentError(gf, continuousValues); + auto errorFunc = [&continuousValues](const auto& pair) { + return pair.first ? pair.first->error(continuousValues) + pair.second + : std::numeric_limits::infinity(); }; DecisionTree error_tree(factors_, errorFunc); return error_tree; } /* *******************************************************************************/ -double HybridGaussianFactor::error(const HybridValues &values) const { +double HybridGaussianFactor::error(const HybridValues& values) const { // Directly index to get the component, no need to build the whole tree. - const sharedFactor gf = factors_(values.discrete()); - return PotentiallyPrunedComponentError(gf, values.continuous()); + const auto pair = factors_(values.discrete()); + return pair.first ? pair.first->error(values.continuous()) + pair.second + : std::numeric_limits::infinity(); } } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index e5a5754094..48a8bf48de 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -57,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; @@ -66,14 +67,12 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { /// typedef for Decision Tree of Gaussian factors and arbitrary value. using FactorValuePairs = DecisionTree; - /// typedef for Decision Tree of Gaussian factors. - using Factors = DecisionTree; - private: +private: /// Decision tree of Gaussian factors indexed by discrete keys. - Factors factors_; + FactorValuePairs factors_; - public: +public: /// @name Constructors /// @{ @@ -110,10 +109,10 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { * The value ϕ(x,M) for the factor is again ϕ_m(x) + E_m. * * @param discreteKeys Discrete variables and their cardinalities. - * @param factors The decision tree of Gaussian factor/scalar pairs. + * @param factorPairs The decision tree of Gaussian factor/scalar pairs. */ HybridGaussianFactor(const DiscreteKeys &discreteKeys, - const FactorValuePairs &factors); + const FactorValuePairs &factorPairs); /// @} /// @name Testable @@ -121,25 +120,16 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { 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 /// @{ /// Get factor at a given discrete assignment. - sharedFactor operator()(const DiscreteValues &assignment) const; - - /** - * @brief Combine the Gaussian Factor Graphs in `sum` and `this` while - * maintaining the original tree structure. - * - * @param sum Decision Tree of Gaussian Factor Graphs indexed by the - * variables. - * @return Sum - */ - GaussianFactorGraphTree add(const GaussianFactorGraphTree &sum) const; + GaussianFactorValuePair operator()(const DiscreteValues &assignment) const; /** * @brief Compute error of the HybridGaussianFactor as a tree. @@ -148,8 +138,8 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { * @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. @@ -158,36 +148,27 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { double error(const HybridValues &values) const override; /// Getter for GaussianFactor decision tree - const Factors &factors() const { return factors_; } - - /// Add HybridNonlinearFactor to a Sum, syntactic sugar. - friend GaussianFactorGraphTree &operator+=( - GaussianFactorGraphTree &sum, const HybridGaussianFactor &factor) { - sum = factor.add(sum); - return sum; - } - /// @} - - protected: + const FactorValuePairs &factors() const { return factors_; } /** * @brief Helper function to return factors and functional to create a * DecisionTree of Gaussian Factor Graphs. * - * @return GaussianFactorGraphTree + * @return HybridGaussianProductFactor */ - GaussianFactorGraphTree asGaussianFactorGraphTree() const; + virtual HybridGaussianProductFactor asProductFactor() const; + + /// @} - 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 * value in the `b` vector as an additional row. * * @param factors DecisionTree of GaussianFactors and arbitrary scalars. - * Gaussian factor in factors. - * @return HybridGaussianFactor::Factors + * @return FactorValuePairs */ - static Factors augment(const FactorValuePairs &factors); + static FactorValuePairs augment(const FactorValuePairs &factors); /// Helper struct to assist private constructor below. struct ConstructorHelper; diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index d4fb5833b6..26cdb8dd2c 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -39,8 +39,8 @@ #include #include #include +#include "gtsam/discrete/DiscreteValues.h" -#include #include #include #include @@ -53,15 +53,19 @@ namespace gtsam { /// Specialize EliminateableFactorGraph for HybridGaussianFactorGraph: template class EliminateableFactorGraph; +using std::dynamic_pointer_cast; using OrphanWrapper = BayesTreeOrphanWrapper; +using Result = + std::pair, GaussianFactor::shared_ptr>; +using ResultTree = DecisionTree>; -using std::dynamic_pointer_cast; +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()) + "."); } @@ -78,28 +82,55 @@ const Ordering HybridOrdering(const HybridGaussianFactorGraph &graph) { static void printFactor(const std::shared_ptr &factor, const DiscreteValues &assignment, const KeyFormatter &keyFormatter) { - if (auto hgf = std::dynamic_pointer_cast(factor)) { - hgf->operator()(assignment) - ->print("HybridGaussianFactor, component:", keyFormatter); - } else if (auto gf = std::dynamic_pointer_cast(factor)) { + if (auto hgf = dynamic_pointer_cast(factor)) { + if (assignment.empty()) + hgf->print("HybridGaussianFactor:", keyFormatter); + else + hgf->operator()(assignment) + .first->print("HybridGaussianFactor, component:", keyFormatter); + } else if (auto gf = dynamic_pointer_cast(factor)) { factor->print("GaussianFactor:\n", keyFormatter); - } else if (auto df = std::dynamic_pointer_cast(factor)) { + + } else if (auto df = dynamic_pointer_cast(factor)) { factor->print("DiscreteFactor:\n", keyFormatter); - } else if (auto hc = std::dynamic_pointer_cast(factor)) { + } else if (auto hc = dynamic_pointer_cast(factor)) { if (hc->isContinuous()) { factor->print("GaussianConditional:\n", keyFormatter); } else if (hc->isDiscrete()) { factor->print("DiscreteConditional:\n", keyFormatter); } else { - hc->asHybrid() - ->choose(assignment) - ->print("HybridConditional, component:\n", keyFormatter); + if (assignment.empty()) + hc->print("HybridConditional:", keyFormatter); + else + hc->asHybrid() + ->choose(assignment) + ->print("HybridConditional, component:\n", keyFormatter); } } else { factor->print("Unknown factor type\n", keyFormatter); } } +/* ************************************************************************ */ +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]; + if (factor == nullptr) { + std::cout << "Factor " << i << ": nullptr\n"; + continue; + } + // Print the factor + std::cout << "Factor " << i << "\n"; + printFactor(factor, {}, keyFormatter); + std::cout << "\n"; + } + std::cout.flush(); +} + /* ************************************************************************ */ void HybridGaussianFactorGraph::printErrors( const HybridValues &values, const std::string &str, @@ -128,42 +159,27 @@ void HybridGaussianFactorGraph::printErrors( } /* ************************************************************************ */ -static GaussianFactorGraphTree addGaussian( - const GaussianFactorGraphTree &gfgTree, - const GaussianFactor::shared_ptr &factor) { - // If the decision tree is not initialized, then initialize it. - if (gfgTree.empty()) { - GaussianFactorGraph result{factor}; - return GaussianFactorGraphTree(result); - } else { - auto add = [&factor](const GaussianFactorGraph &graph) { - auto result = graph; - result.push_back(factor); - return result; - }; - return gfgTree.apply(add); - } -} - -/* ************************************************************************ */ -// TODO(dellaert): it's probably more efficient to first collect the discrete -// keys, and then loop over all assignments to populate a vector. -GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const { - GaussianFactorGraphTree result; - - for (auto &f : factors_) { - // TODO(dellaert): just use a virtual method defined in HybridFactor. - if (auto gf = dynamic_pointer_cast(f)) { - result = addGaussian(result, gf); +HybridGaussianProductFactor HybridGaussianFactorGraph::collectProductFactor() + const { + HybridGaussianProductFactor result; + + 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 + } else if (auto gf = dynamic_pointer_cast(f)) { + result += gf; + } else if (auto gc = dynamic_pointer_cast(f)) { + result += gc; } else if (auto gmf = dynamic_pointer_cast(f)) { - result = gmf->add(result); + result += *gmf; } else if (auto gm = dynamic_pointer_cast(f)) { - result = gm->add(result); + result += *gm; // handled above already? } else if (auto hc = dynamic_pointer_cast(f)) { if (auto gm = hc->asHybrid()) { - result = gm->add(result); + result += *gm; } else if (auto g = hc->asGaussian()) { - result = addGaussian(result, g); + result += g; } else { // Has to be discrete. // TODO(dellaert): in C++20, we can use std::visit. @@ -176,7 +192,7 @@ GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const { } else { // TODO(dellaert): there was an unattributed comment here: We need to // handle the case where the object is actually an BayesTreeOrphanWrapper! - throwRuntimeError("gtsam::assembleGraphTree", f); + throwRuntimeError("gtsam::collectProductFactor", f); } } @@ -207,25 +223,6 @@ continuousElimination(const HybridGaussianFactorGraph &factors, return {std::make_shared(result.first), result.second}; } -/* ************************************************************************ */ -/** - * @brief Exponentiate (not necessarily normalized) negative log-values, - * normalize, and then return as AlgebraicDecisionTree. - * - * @param logValues DecisionTree of (unnormalized) log values. - * @return AlgebraicDecisionTree - */ -static AlgebraicDecisionTree probabilitiesFromNegativeLogValues( - const AlgebraicDecisionTree &logValues) { - // Perform normalization - double min_log = logValues.min(); - AlgebraicDecisionTree probabilities = DecisionTree( - logValues, [&min_log](const double x) { return exp(-(x - min_log)); }); - probabilities = probabilities.normalize(probabilities.sum()); - - return probabilities; -} - /* ************************************************************************ */ static std::pair> discreteElimination(const HybridGaussianFactorGraph &factors, @@ -238,20 +235,14 @@ 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 logProbability = - [&](const GaussianFactor::shared_ptr &factor) -> double { - // If the factor is null, it is has been pruned hence return ∞ - // so that the exp(-∞)=0. - if (!factor) return std::numeric_limits::infinity(); - return factor->error(VectorValues()); + 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; + return exp(-scalar - factor->error(kEmpty)); }; - AlgebraicDecisionTree logProbabilities = - DecisionTree(gmf->factors(), logProbability); - - AlgebraicDecisionTree probabilities = - probabilitiesFromNegativeLogValues(logProbabilities); - dfg.emplace_shared(gmf->discreteKeys(), - probabilities); + DecisionTree potentials(gmf->factors(), potential); + dfg.emplace_shared(gmf->discreteKeys(), potentials); } else if (auto orphan = dynamic_pointer_cast(f)) { // Ignore orphaned clique. @@ -272,24 +263,6 @@ discreteElimination(const HybridGaussianFactorGraph &factors, } /* ************************************************************************ */ -// If any GaussianFactorGraph in the decision tree contains a nullptr, convert -// that leaf to an empty GaussianFactorGraph. Needed since the DecisionTree will -// otherwise create a GFG with a single (null) factor, -// which doesn't register as null. -GaussianFactorGraphTree removeEmpty(const GaussianFactorGraphTree &sum) { - auto emptyGaussian = [](const GaussianFactorGraph &graph) { - bool hasNull = - std::any_of(graph.begin(), graph.end(), - [](const GaussianFactor::shared_ptr &ptr) { return !ptr; }); - return hasNull ? GaussianFactorGraph() : graph; - }; - return GaussianFactorGraphTree(sum, emptyGaussian); -} - -/* ************************************************************************ */ -using Result = std::pair, - HybridGaussianFactor::sharedFactor>; - /** * Compute the probability p(μ;m) = exp(-error(μ;m)) * sqrt(det(2π Σ_m) * from the residual error ||b||^2 at the mean μ. @@ -297,50 +270,50 @@ using Result = std::pair, * depends on the discrete separator if present. */ static std::shared_ptr createDiscreteFactor( - const DecisionTree &eliminationResults, + const ResultTree& eliminationResults, const DiscreteKeys &discreteSeparator) { - auto negLogProbability = [&](const Result &pair) -> double { - const auto &[conditional, factor] = pair; - static const VectorValues kEmpty; - // If the factor is null, it has been pruned, hence return ∞ - // so that the exp(-∞)=0. - if (!factor) return std::numeric_limits::infinity(); - - // Negative logspace version of: - // exp(-factor->error(kEmpty)) / conditional->normalizationConstant(); - // = exp(-factor->error(kEmpty)) * \sqrt{|2πΣ|}; - // log = -(-factor->error(kEmpty) + log(\sqrt{|2πΣ|})) - // = factor->error(kEmpty) - log(\sqrt{|2πΣ|}) - // negLogConstant gives `-log(k)` - // which is `-log(k) = log(1/k) = log(\sqrt{|2πΣ|})`. - return factor->error(kEmpty) - conditional->negLogConstant(); + auto potential = [&](const auto &pair) -> double { + const auto& [conditional, factor] = pair.first; + const double scalar = pair.second; + if (conditional && factor) { + // `error` has the following contributions: + // - the scalar is the sum of all mode-dependent constants + // - factor->error(kempty) is the error remaining after elimination + // - negLogK is what is given to the conditional to normalize + const double negLogK = conditional->negLogConstant(); + const double error = scalar + factor->error(kEmpty) - negLogK; + return exp(-error); + } else if (!conditional && !factor) { + // If the factor is null, it has been pruned, hence return potential of + // zero + return 0.0; + } else { + throw std::runtime_error("createDiscreteFactor has mixed NULLs"); + } }; - AlgebraicDecisionTree negLogProbabilities( - DecisionTree(eliminationResults, negLogProbability)); - AlgebraicDecisionTree probabilities = - probabilitiesFromNegativeLogValues(negLogProbabilities); - - return std::make_shared(discreteSeparator, probabilities); + DecisionTree potentials(eliminationResults, potential); + return std::make_shared(discreteSeparator, potentials); } +/* *******************************************************************************/ // Create HybridGaussianFactor on the separator, taking care to correct // for conditional constants. static std::shared_ptr createHybridGaussianFactor( - const DecisionTree &eliminationResults, + const ResultTree &eliminationResults, const DiscreteKeys &discreteSeparator) { // Correct for the normalization constant used up by the conditional - auto correct = [&](const Result &pair) -> GaussianFactorValuePair { - const auto &[conditional, factor] = pair; - if (factor) { - auto hf = std::dynamic_pointer_cast(factor); - if (!hf) throw std::runtime_error("Expected HessianFactor!"); - // Add 2.0 term since the constant term will be premultiplied by 0.5 - // as per the Hessian definition, - // and negative since we want log(k) - hf->constantTerm() += -2.0 * conditional->negLogConstant(); + auto correct = [&](const auto &pair) -> GaussianFactorValuePair { + const auto &[conditional, factor] = pair.first; + const double scalar = pair.second; + if (conditional && factor) { + const double negLogK = conditional->negLogConstant(); + return {factor, scalar - negLogK}; + } else if (!conditional && !factor) { + return {nullptr, std::numeric_limits::infinity()}; + } else { + throw std::runtime_error("createHybridGaussianFactors has mixed NULLs"); } - return {factor, 0.0}; }; DecisionTree newFactors(eliminationResults, correct); @@ -364,32 +337,40 @@ HybridGaussianFactorGraph::eliminate(const Ordering &keys) const { DiscreteKeys discreteSeparator = GetDiscreteKeys(*this); // Collect all the factors to create a set of Gaussian factor graphs in a - // decision tree indexed by all discrete keys involved. - GaussianFactorGraphTree factorGraphTree = assembleGraphTree(); + // decision tree indexed by all discrete keys involved. Just like any hybrid + // factor, every assignment also has a scalar error, in this case the sum of + // all errors in the graph. This error is assignment-specific and accounts for + // any difference in noise models used. + HybridGaussianProductFactor productFactor = collectProductFactor(); // Convert factor graphs with a nullptr to an empty factor graph. // This is done after assembly since it is non-trivial to keep track of which // FG has a nullptr as we're looping over the factors. - factorGraphTree = removeEmpty(factorGraphTree); + auto prunedProductFactor = productFactor.removeEmpty(); // This is the elimination method on the leaf nodes bool someContinuousLeft = false; - auto eliminate = [&](const GaussianFactorGraph &graph) -> Result { + auto eliminate = [&](const std::pair& pair) + -> std::pair { + const auto& [graph, scalar] = pair; + if (graph.empty()) { - return {nullptr, nullptr}; + return {{nullptr, nullptr}, 0.0}; } // Expensive elimination of product factor. - auto result = EliminatePreferCholesky(graph, keys); + auto result = + EliminatePreferCholesky(graph, keys); /// <<<<<< MOST COMPUTE IS HERE // Record whether there any continuous variables left someContinuousLeft |= !result.second->empty(); - return result; + // We pass on the scalar unmodified. + return {result, scalar}; }; // Perform elimination! - DecisionTree eliminationResults(factorGraphTree, eliminate); + ResultTree eliminationResults(prunedProductFactor, eliminate); // If there are no more continuous parents we create a DiscreteFactor with the // error for each discrete choice. Otherwise, create a HybridGaussianFactor @@ -401,7 +382,7 @@ HybridGaussianFactorGraph::eliminate(const Ordering &keys) const { // Create the HybridGaussianConditional from the conditionals HybridGaussianConditional::Conditionals conditionals( - eliminationResults, [](const Result &pair) { return pair.first; }); + eliminationResults, [](const auto& pair) { return pair.first.first; }); auto hybridGaussian = std::make_shared( discreteSeparator, conditionals); @@ -484,6 +465,7 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors, } else if (hybrid_factor->isHybrid()) { only_continuous = false; only_discrete = false; + break; } } else if (auto cont_factor = std::dynamic_pointer_cast(factor)) { @@ -520,10 +502,11 @@ AlgebraicDecisionTree HybridGaussianFactorGraph::errorTree( } else if (auto df = std::dynamic_pointer_cast(factor)) { // If discrete, just add its errorTree as well result = result + df->errorTree(); + } else if (auto gf = dynamic_pointer_cast(factor)) { + // For a continuous only factor, just add its error + result = result + gf->error(continuousValues); } else { - // Everything else is a continuous only factor - HybridValues hv(continuousValues, DiscreteValues()); - result = result + factor->error(hv); // NOTE: yes, you can add constants + throwRuntimeError("HybridGaussianFactorGraph::errorTree", factor); } } return result; @@ -557,9 +540,14 @@ GaussianFactorGraph HybridGaussianFactorGraph::choose( } else if (auto gc = std::dynamic_pointer_cast(f)) { gfg.push_back(gf); } else if (auto hgf = std::dynamic_pointer_cast(f)) { - gfg.push_back((*hgf)(assignment)); + gfg.push_back((*hgf)(assignment).first); } else if (auto hgc = dynamic_pointer_cast(f)) { gfg.push_back((*hgc)(assignment)); + } else if (auto hc = dynamic_pointer_cast(f)) { + if (auto gc = hc->asGaussian()) + gfg.push_back(gc); + else if (auto hgc = hc->asHybrid()) + gfg.push_back((*hgc)(assignment)); } else { continue; } diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index a5130ca086..d72e814892 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -145,10 +145,9 @@ class GTSAM_EXPORT HybridGaussianFactorGraph /// @name Testable /// @{ - // TODO(dellaert): customize print and equals. - // 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. @@ -218,7 +217,7 @@ class GTSAM_EXPORT HybridGaussianFactorGraph * one for A and one for B. The leaves of the tree will be the Gaussian * factors that have only continuous keys. */ - GaussianFactorGraphTree assembleGraphTree() const; + HybridGaussianProductFactor collectProductFactor() const; /** * @brief Eliminate the given continuous keys. diff --git a/gtsam/hybrid/HybridGaussianProductFactor.cpp b/gtsam/hybrid/HybridGaussianProductFactor.cpp new file mode 100644 index 0000000000..2e95ea8d11 --- /dev/null +++ b/gtsam/hybrid/HybridGaussianProductFactor.cpp @@ -0,0 +1,90 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file HybridGaussianProductFactor.h + * @date Oct 2, 2024 + * @author Frank Dellaert + * @author Varun Agrawal + */ + +#include +#include +#include +#include +#include + +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; + auto printer = [&](const Y& y) { + if (keys.empty()) keys = y.first.keys(); + return "Graph of size " + std::to_string(y.first.size()) + + ", scalar sum: " + std::to_string(y.second); + }; + 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; + } +} + +HybridGaussianProductFactor HybridGaussianProductFactor::removeEmpty() const { + auto emptyGaussian = [](const Y& y) { + bool hasNull = + std::any_of(y.first.begin(), + y.first.end(), + [](const GaussianFactor::shared_ptr& ptr) { return !ptr; }); + return hasNull ? Y{GaussianFactorGraph(), 0.0} : y; + }; + return {Base(*this, emptyGaussian)}; +} + +} // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianProductFactor.h b/gtsam/hybrid/HybridGaussianProductFactor.h new file mode 100644 index 0000000000..9c2aee74ab --- /dev/null +++ b/gtsam/hybrid/HybridGaussianProductFactor.h @@ -0,0 +1,129 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file HybridGaussianProductFactor.h + * @date Oct 2, 2024 + * @author Frank Dellaert + * @author Varun Agrawal + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +class HybridGaussianFactor; + +/// Alias for DecisionTree of GaussianFactorGraphs and their scalar sums +class HybridGaussianProductFactor + : public DecisionTree> { + public: + using Y = std::pair; + using Base = DecisionTree; + + /// @name Constructors + /// @{ + + /// Default constructor + HybridGaussianProductFactor() = default; + + /** + * @brief Construct from a single factor + * @tparam FACTOR Factor type + * @param factor Shared pointer to the factor + */ + template + HybridGaussianProductFactor(const std::shared_ptr& factor) + : Base(Y{GaussianFactorGraph{factor}, 0.0}) {} + + /** + * @brief Construct from DecisionTree + * @param tree Decision tree to construct from + */ + HybridGaussianProductFactor(Base&& tree) : Base(std::move(tree)) {} + + ///@} + + /// @name Operators + ///@{ + + /// Add GaussianFactor into HybridGaussianProductFactor + HybridGaussianProductFactor operator+( + const GaussianFactor::shared_ptr& factor) const; + + /// Add HybridGaussianFactor into HybridGaussianProductFactor + HybridGaussianProductFactor operator+( + const HybridGaussianFactor& factor) const; + + /// Add-assign operator for GaussianFactor + HybridGaussianProductFactor& operator+=( + const GaussianFactor::shared_ptr& factor); + + /// Add-assign operator for HybridGaussianFactor + HybridGaussianProductFactor& operator+=(const HybridGaussianFactor& factor); + + ///@} + + /// @name Testable + /// @{ + + /** + * @brief Print the HybridGaussianProductFactor + * @param s Optional string to prepend + * @param formatter Optional key formatter + */ + void print(const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const; + + /** + * @brief Check if this HybridGaussianProductFactor is equal to another + * @param other The other HybridGaussianProductFactor to compare with + * @param tol Tolerance for floating point comparisons + * @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; + }); + } + + /// @} + + /// @name Other methods + ///@{ + + /** + * @brief Remove empty GaussianFactorGraphs from the decision tree + * @return A new HybridGaussianProductFactor with empty GaussianFactorGraphs + * removed + * + * If any GaussianFactorGraph in the decision tree contains a nullptr, convert + * that leaf to an empty GaussianFactorGraph with zero scalar sum. This is + * needed because the DecisionTree will otherwise create a GaussianFactorGraph + * with a single (null) factor, which doesn't register as null. + */ + HybridGaussianProductFactor removeEmpty() const; + + ///@} +}; + +// Testable traits +template <> +struct traits + : public Testable {}; + +} // namespace gtsam diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp index 0e7e9c692d..2f5031cf28 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp @@ -210,4 +210,16 @@ AlgebraicDecisionTree HybridNonlinearFactorGraph::errorTree( return result; } +/* ************************************************************************ */ +AlgebraicDecisionTree HybridNonlinearFactorGraph::discretePosterior( + const Values& continuousValues) const { + AlgebraicDecisionTree errors = this->errorTree(continuousValues); + AlgebraicDecisionTree p = errors.apply([](double error) { + // NOTE: The 0.5 term is handled by each factor + return exp(-error); + }); + return p / p.sum(); +} + +/* ************************************************************************ */ } // namespace gtsam diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.h b/gtsam/hybrid/HybridNonlinearFactorGraph.h index 53920a4aad..f79f7b4524 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.h +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.h @@ -103,6 +103,19 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph { */ AlgebraicDecisionTree errorTree(const Values& values) const; + /** + * @brief Computer posterior P(M|X=x) when all continuous values X are given. + * This is efficient as this simply takes -exp(.) of errorTree and normalizes. + * + * @note Not a DiscreteConditional as the cardinalities of the DiscreteKeys, + * which we would need, are hard to recover. + * + * @param continuousValues Continuous values x to condition on. + * @return DecisionTreeFactor + */ + AlgebraicDecisionTree discretePosterior( + const Values& continuousValues) const; + /// @} }; diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index 3256f5686c..14bef5fbb4 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -60,17 +60,6 @@ double prob_m_z(double mu0, double mu1, double sigma0, double sigma1, return p1 / (p0 + p1); }; -/// Given \phi(m;z)\phi(m) use eliminate to obtain P(m|z). -DiscreteConditional SolveHFG(const HybridGaussianFactorGraph &hfg) { - return *hfg.eliminateSequential()->at(0)->asDiscrete(); -} - -/// Given p(z,m) and z, convert to HFG and solve. -DiscreteConditional SolveHBN(const HybridBayesNet &hbn, double z) { - VectorValues given{{Z(0), Vector1(z)}}; - return SolveHFG(hbn.toFactorGraph(given)); -} - /* * Test a Gaussian Mixture Model P(m)p(z|m) with same sigma. * The posterior, as a function of z, should be a sigmoid function. @@ -88,7 +77,9 @@ TEST(GaussianMixture, GaussianMixtureModel) { // At the halfway point between the means, we should get P(m|z)=0.5 double midway = mu1 - mu0; - auto pMid = SolveHBN(gmm, midway); + auto eliminationResult = + gmm.toFactorGraph({{Z(0), Vector1(midway)}}).eliminateSequential(); + auto pMid = *eliminationResult->at(0)->asDiscrete(); EXPECT(assert_equal(DiscreteConditional(m, "60/40"), pMid)); // Everywhere else, the result should be a sigmoid. @@ -97,7 +88,9 @@ TEST(GaussianMixture, GaussianMixtureModel) { const double expected = prob_m_z(mu0, mu1, sigma, sigma, z); // Workflow 1: convert HBN to HFG and solve - auto posterior1 = SolveHBN(gmm, z); + auto eliminationResult1 = + gmm.toFactorGraph({{Z(0), Vector1(z)}}).eliminateSequential(); + auto posterior1 = *eliminationResult1->at(0)->asDiscrete(); EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8); // Workflow 2: directly specify HFG and solve @@ -105,7 +98,8 @@ TEST(GaussianMixture, GaussianMixtureModel) { hfg1.emplace_shared( m, std::vector{Gaussian(mu0, sigma, z), Gaussian(mu1, sigma, z)}); hfg1.push_back(mixing); - auto posterior2 = SolveHFG(hfg1); + auto eliminationResult2 = hfg1.eliminateSequential(); + auto posterior2 = *eliminationResult2->at(0)->asDiscrete(); EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8); } } @@ -128,7 +122,23 @@ TEST(GaussianMixture, GaussianMixtureModel2) { // We get zMax=3.1333 by finding the maximum value of the function, at which // point the mode m==1 is about twice as probable as m==0. double zMax = 3.133; - auto pMax = SolveHBN(gmm, zMax); + const VectorValues vv{{Z(0), Vector1(zMax)}}; + auto gfg = gmm.toFactorGraph(vv); + + // Equality of posteriors asserts that the elimination is correct (same ratios + // for all modes) + const auto& expectedDiscretePosterior = gmm.discretePosterior(vv); + EXPECT(assert_equal(expectedDiscretePosterior, gfg.discretePosterior(vv))); + + // Eliminate the graph! + auto eliminationResultMax = gfg.eliminateSequential(); + + // Equality of posteriors asserts that the elimination is correct (same ratios + // for all modes) + EXPECT(assert_equal(expectedDiscretePosterior, + eliminationResultMax->discretePosterior(vv))); + + auto pMax = *eliminationResultMax->at(0)->asDiscrete(); EXPECT(assert_equal(DiscreteConditional(m, "42/58"), pMax, 1e-4)); // Everywhere else, the result should be a bell curve like function. @@ -137,7 +147,9 @@ TEST(GaussianMixture, GaussianMixtureModel2) { const double expected = prob_m_z(mu0, mu1, sigma0, sigma1, z); // Workflow 1: convert HBN to HFG and solve - auto posterior1 = SolveHBN(gmm, z); + auto eliminationResult1 = + gmm.toFactorGraph({{Z(0), Vector1(z)}}).eliminateSequential(); + auto posterior1 = *eliminationResult1->at(0)->asDiscrete(); EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8); // Workflow 2: directly specify HFG and solve @@ -145,11 +157,11 @@ TEST(GaussianMixture, GaussianMixtureModel2) { hfg.emplace_shared( m, std::vector{Gaussian(mu0, sigma0, z), Gaussian(mu1, sigma1, z)}); hfg.push_back(mixing); - auto posterior2 = SolveHFG(hfg); + auto eliminationResult2 = hfg.eliminateSequential(); + auto posterior2 = *eliminationResult2->at(0)->asDiscrete(); EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8); } } - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 1d22b3d73e..d4192cb712 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -18,9 +18,12 @@ * @date December 2021 */ +#include +#include #include #include #include +#include #include #include "Switching.h" @@ -28,6 +31,7 @@ // Include for test suite #include +#include using namespace std; using namespace gtsam; @@ -113,7 +117,7 @@ TEST(HybridBayesNet, EvaluatePureDiscrete) { } /* ****************************************************************************/ -// Test creation of a tiny hybrid Bayes net. +// Test API for a tiny hybrid Bayes net. TEST(HybridBayesNet, Tiny) { auto bayesNet = tiny::createHybridBayesNet(); // P(z|x,mode)P(x)P(mode) EXPECT_LONGS_EQUAL(3, bayesNet.size()); @@ -189,6 +193,20 @@ TEST(HybridBayesNet, Tiny) { auto fg = bayesNet.toFactorGraph({{Z(0), Vector1(5.0)}}); EXPECT_LONGS_EQUAL(3, fg.size()); + // Create the product factor for eliminating x0: + HybridGaussianFactorGraph factors_x0; + factors_x0.push_back(fg.at(0)); + factors_x0.push_back(fg.at(1)); + auto productFactor = factors_x0.collectProductFactor(); + + // Check that scalars are 0 and 1.79 + EXPECT_DOUBLES_EQUAL(0.0, productFactor({{M(0), 0}}).second, 1e-9); + EXPECT_DOUBLES_EQUAL(1.791759, productFactor({{M(0), 1}}).second, 1e-5); + + // Call eliminate and check scalar: + auto result = factors_x0.eliminate({X(0)}); + auto df = std::dynamic_pointer_cast(result.second); + // Check that the ratio of probPrime to evaluate is the same for all modes. std::vector ratio(2); ratio[0] = std::exp(-fg.error(zero)) / bayesNet.evaluate(zero); diff --git a/gtsam/hybrid/tests/testHybridBayesTree.cpp b/gtsam/hybrid/tests/testHybridBayesTree.cpp index 81b257c32e..319c6d52b0 100644 --- a/gtsam/hybrid/tests/testHybridBayesTree.cpp +++ b/gtsam/hybrid/tests/testHybridBayesTree.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include "Switching.h" @@ -28,9 +29,327 @@ using namespace std; using namespace gtsam; -using noiseModel::Isotropic; +using symbol_shorthand::D; using symbol_shorthand::M; using symbol_shorthand::X; +using symbol_shorthand::Y; + +static const DiscreteKey m0(M(0), 2), m1(M(1), 2), m2(M(2), 2), m3(M(3), 2); + +/* ************************************************************************* */ +TEST(HybridGaussianFactorGraph, EliminateMultifrontal) { + // Test multifrontal elimination + HybridGaussianFactorGraph hfg; + + // Add priors on x0 and c1 + hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); + hfg.add(DecisionTreeFactor(m1, {2, 8})); + + Ordering ordering; + ordering.push_back(X(0)); + auto result = hfg.eliminatePartialMultifrontal(ordering); + + EXPECT_LONGS_EQUAL(result.first->size(), 1); + EXPECT_LONGS_EQUAL(result.second->size(), 1); +} + +/* ************************************************************************* */ +namespace two { +std::vector components(Key key) { + return {std::make_shared(key, I_3x3, Z_3x1), + std::make_shared(key, I_3x3, Vector3::Ones())}; +} +} // namespace two + +/* ************************************************************************* */ +TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) { + HybridGaussianFactorGraph hfg; + + hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); + hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); + + hfg.add(HybridGaussianFactor(m1, two::components(X(1)))); + + hfg.add(DecisionTreeFactor(m1, {2, 8})); + // TODO(Varun) Adding extra discrete variable not connected to continuous + // variable throws segfault + // hfg.add(DecisionTreeFactor({m1, m2, "1 2 3 4")); + + HybridBayesTree::shared_ptr result = hfg.eliminateMultifrontal(); + + // The bayes tree should have 3 cliques + EXPECT_LONGS_EQUAL(3, result->size()); +} + +/* ************************************************************************* */ +TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) { + HybridGaussianFactorGraph hfg; + + // Prior on x0 + hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); + // Factor between x0-x1 + hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); + + // Hybrid factor P(x1|c1) + hfg.add(HybridGaussianFactor(m1, two::components(X(1)))); + // Prior factor on c1 + hfg.add(DecisionTreeFactor(m1, {2, 8})); + + // Get a constrained ordering keeping c1 last + auto ordering_full = HybridOrdering(hfg); + + // Returns a Hybrid Bayes Tree with distribution P(x0|x1)P(x1|c1)P(c1) + HybridBayesTree::shared_ptr hbt = hfg.eliminateMultifrontal(ordering_full); + + EXPECT_LONGS_EQUAL(3, hbt->size()); +} + +/* ************************************************************************* */ +// Check assembling the Bayes Tree roots after we do partial elimination +TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) { + HybridGaussianFactorGraph hfg; + + hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); + hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1)); + + hfg.add(HybridGaussianFactor(m0, two::components(X(0)))); + hfg.add(HybridGaussianFactor(m1, two::components(X(2)))); + + hfg.add(DecisionTreeFactor({m1, m2}, "1 2 3 4")); + + hfg.add(JacobianFactor(X(3), I_3x3, X(4), -I_3x3, Z_3x1)); + hfg.add(JacobianFactor(X(4), I_3x3, X(5), -I_3x3, Z_3x1)); + + hfg.add(HybridGaussianFactor(m3, two::components(X(3)))); + hfg.add(HybridGaussianFactor(m2, two::components(X(5)))); + + auto ordering_full = + Ordering::ColamdConstrainedLast(hfg, {M(0), M(1), M(2), M(3)}); + + const auto [hbt, remaining] = hfg.eliminatePartialMultifrontal(ordering_full); + + // 9 cliques in the bayes tree and 0 remaining variables to eliminate. + EXPECT_LONGS_EQUAL(9, hbt->size()); + EXPECT_LONGS_EQUAL(0, remaining->size()); + + /* + (Fan) Explanation: the Junction tree will need to re-eliminate to get to the + marginal on X(1), which is not possible because it involves eliminating + discrete before continuous. The solution to this, however, is in Murphy02. + TLDR is that this is 1. expensive and 2. inexact. nevertheless it is doable. + And I believe that we should do this. + */ +} + +/* ************************************************************************* */ +void dotPrint(const HybridGaussianFactorGraph::shared_ptr& hfg, + const HybridBayesTree::shared_ptr& hbt, + const Ordering& ordering) { + DotWriter dw; + dw.positionHints['c'] = 2; + dw.positionHints['x'] = 1; + std::cout << hfg->dot(DefaultKeyFormatter, dw); + std::cout << "\n"; + hbt->dot(std::cout); + + std::cout << "\n"; + std::cout << hfg->eliminateSequential(ordering)->dot(DefaultKeyFormatter, dw); +} + +/* ************************************************************************* */ +// TODO(fan): make a graph like Varun's paper one +TEST(HybridGaussianFactorGraph, Switching) { + auto N = 12; + auto hfg = makeSwitchingChain(N); + + // X(5) will be the center, X(1-4), X(6-9) + // X(3), X(7) + // X(2), X(8) + // X(1), X(4), X(6), X(9) + // M(5) will be the center, M(1-4), M(6-8) + // M(3), M(7) + // M(1), M(4), M(2), M(6), M(8) + // auto ordering_full = + // Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7), + // X(5), + // M(1), M(4), M(2), M(6), M(8), M(3), M(7), M(5)}); + KeyVector ordering; + + { + 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); + }); + + auto [ndX, lvls] = makeBinaryOrdering(ordX); + std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering)); + // TODO(dellaert): this has no effect! + for (auto& l : lvls) { + l = -l; + } + } + { + 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::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering)); + const auto [ndC, lvls] = makeBinaryOrdering(ordC); + std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering)); + } + auto ordering_full = Ordering(ordering); + + const auto [hbt, remaining] = + hfg->eliminatePartialMultifrontal(ordering_full); + + // 12 cliques in the bayes tree and 0 remaining variables to eliminate. + EXPECT_LONGS_EQUAL(12, hbt->size()); + EXPECT_LONGS_EQUAL(0, remaining->size()); +} + +/* ************************************************************************* */ +// TODO(fan): make a graph like Varun's paper one +TEST(HybridGaussianFactorGraph, SwitchingISAM) { + auto N = 11; + auto hfg = makeSwitchingChain(N); + + // X(5) will be the center, X(1-4), X(6-9) + // X(3), X(7) + // X(2), X(8) + // X(1), X(4), X(6), X(9) + // M(5) will be the center, M(1-4), M(6-8) + // M(3), M(7) + // M(1), M(4), M(2), M(6), M(8) + // auto ordering_full = + // Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7), + // X(5), + // M(1), M(4), M(2), M(6), M(8), M(3), M(7), M(5)}); + KeyVector ordering; + + { + 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); + }); + + auto [ndX, lvls] = makeBinaryOrdering(ordX); + std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering)); + // TODO(dellaert): this has no effect! + for (auto& l : lvls) { + l = -l; + } + } + { + 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::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering)); + const auto [ndC, lvls] = makeBinaryOrdering(ordC); + std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering)); + } + auto ordering_full = Ordering(ordering); + + const auto [hbt, remaining] = + hfg->eliminatePartialMultifrontal(ordering_full); + + auto new_fg = makeSwitchingChain(12); + auto isam = HybridGaussianISAM(*hbt); + + // Run an ISAM update. + HybridGaussianFactorGraph factorGraph; + factorGraph.push_back(new_fg->at(new_fg->size() - 2)); + factorGraph.push_back(new_fg->at(new_fg->size() - 1)); + isam.update(factorGraph); + + // ISAM should have 12 factors after the last update + EXPECT_LONGS_EQUAL(12, isam.size()); +} + +/* ************************************************************************* */ +TEST(HybridGaussianFactorGraph, SwitchingTwoVar) { + const int N = 7; + auto hfg = makeSwitchingChain(N, X); + hfg->push_back(*makeSwitchingChain(N, Y, D)); + + for (int t = 1; t <= N; t++) { + hfg->add(JacobianFactor(X(t), I_3x3, Y(t), -I_3x3, Vector3(1.0, 0.0, 0.0))); + } + + KeyVector ordering; + + KeyVector naturalX(N); + std::iota(naturalX.begin(), naturalX.end(), 1); + KeyVector ordX; + for (size_t i = 1; i <= N; i++) { + ordX.emplace_back(X(i)); + ordX.emplace_back(Y(i)); + } + + for (size_t i = 1; i <= N - 1; i++) { + ordX.emplace_back(M(i)); + } + for (size_t i = 1; i <= N - 1; i++) { + ordX.emplace_back(D(i)); + } + + { + DotWriter dw; + dw.positionHints['x'] = 1; + dw.positionHints['c'] = 0; + dw.positionHints['d'] = 3; + dw.positionHints['y'] = 2; + // std::cout << hfg->dot(DefaultKeyFormatter, dw); + // std::cout << "\n"; + } + + { + DotWriter dw; + dw.positionHints['y'] = 9; + // dw.positionHints['c'] = 0; + // dw.positionHints['d'] = 3; + dw.positionHints['x'] = 1; + // std::cout << "\n"; + // std::cout << hfg->eliminateSequential(Ordering(ordX)) + // ->dot(DefaultKeyFormatter, dw); + // hfg->eliminateMultifrontal(Ordering(ordX))->dot(std::cout); + } + + Ordering ordering_partial; + for (size_t i = 1; i <= N; i++) { + ordering_partial.emplace_back(X(i)); + ordering_partial.emplace_back(Y(i)); + } + const auto [hbn, remaining] = + hfg->eliminatePartialSequential(ordering_partial); + + EXPECT_LONGS_EQUAL(14, hbn->size()); + EXPECT_LONGS_EQUAL(11, remaining->size()); + + { + DotWriter dw; + dw.positionHints['x'] = 1; + dw.positionHints['c'] = 0; + dw.positionHints['d'] = 3; + dw.positionHints['y'] = 2; + // std::cout << remaining->dot(DefaultKeyFormatter, dw); + // std::cout << "\n"; + } +} /* ****************************************************************************/ // Test multifrontal optimize diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 32d7277a2b..6206e5f459 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -37,8 +37,6 @@ // Include for test suite #include -#include - #include "Switching.h" using namespace std; @@ -517,8 +515,6 @@ TEST(HybridEstimation, CorrectnessViaSampling) { // the normalizing term computed via the Bayes net determinant. const HybridValues sample = bn->sample(&rng); double expected_ratio = compute_ratio(sample); - // regression - EXPECT_DOUBLES_EQUAL(0.728588, expected_ratio, 1e-6); // 3. Do sampling constexpr int num_samples = 10; diff --git a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp index cd9c182cd5..e29c485afd 100644 --- a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp @@ -18,6 +18,8 @@ * @date December 2021 */ +#include +#include #include #include #include @@ -28,9 +30,6 @@ #include #include -#include "gtsam/discrete/DecisionTree.h" -#include "gtsam/discrete/DiscreteKey.h" - // Include for test suite #include @@ -217,30 +216,16 @@ TEST(HybridGaussianConditional, Likelihood2) { // Check the detailed JacobianFactor calculation for mode==1. { // We have a JacobianFactor - const auto gf1 = (*likelihood)(assignment1); + const auto [gf1, _] = (*likelihood)(assignment1); const auto jf1 = std::dynamic_pointer_cast(gf1); CHECK(jf1); - // It has 2 rows, not 1! - CHECK(jf1->rows() == 2); - - // Check that the constant C1 is properly encoded in the JacobianFactor. - const double C1 = - conditionals[1]->negLogConstant() - hybrid_conditional.negLogConstant(); - const double c1 = std::sqrt(2.0 * C1); - Vector expected_unwhitened(2); - expected_unwhitened << 4.9 - 5.0, -c1; - Vector actual_unwhitened = jf1->unweighted_error(vv); - EXPECT(assert_equal(expected_unwhitened, actual_unwhitened)); - - // Make sure the noise model does not touch it. - Vector expected_whitened(2); - expected_whitened << (4.9 - 5.0) / 3.0, -c1; - Vector actual_whitened = jf1->error_vector(vv); - EXPECT(assert_equal(expected_whitened, actual_whitened)); - - // Check that the error is equal to the conditional error: - EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv1), jf1->error(hv1), 1e-8); + // Check that the JacobianFactor error with constants is equal to the + // conditional error: + EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv1), + jf1->error(hv1) + conditionals[1]->negLogConstant() - + hybrid_conditional.negLogConstant(), + 1e-8); } // Check that the ratio of probPrime to evaluate is the same for all modes. diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp index 5ff8c14781..71aac8224b 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp @@ -82,44 +82,29 @@ TEST(HybridGaussianFactor, ConstructorVariants) { } /* ************************************************************************* */ -// "Add" two hybrid factors together. -TEST(HybridGaussianFactor, Sum) { +TEST(HybridGaussianFactor, Keys) { using namespace test_constructor; - DiscreteKey m2(2, 3); + HybridGaussianFactor hybridFactorA(m1, {f10, f11}); + // Check the number of keys matches what we expect + EXPECT_LONGS_EQUAL(3, hybridFactorA.keys().size()); + EXPECT_LONGS_EQUAL(2, hybridFactorA.continuousKeys().size()); + EXPECT_LONGS_EQUAL(1, hybridFactorA.discreteKeys().size()); + DiscreteKey m2(2, 3); auto A3 = Matrix::Zero(2, 3); 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); - - // 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? - // Design review! - HybridGaussianFactor hybridFactorA(m1, {f10, f11}); HybridGaussianFactor hybridFactorB(m2, {f20, f21, f22}); // Check the number of keys matches what we expect - EXPECT_LONGS_EQUAL(3, hybridFactorA.keys().size()); - EXPECT_LONGS_EQUAL(2, hybridFactorA.continuousKeys().size()); - EXPECT_LONGS_EQUAL(1, hybridFactorA.discreteKeys().size()); - - // Create sum of two hybrid factors: it will be a decision tree now on both - // discrete variables m1 and m2: - GaussianFactorGraphTree sum; - sum += hybridFactorA; - sum += hybridFactorB; - - // Let's check that this worked: - Assignment mode; - mode[m1.first] = 1; - mode[m2.first] = 2; - auto actual = sum(mode); - EXPECT(actual.at(0) == f11); - EXPECT(actual.at(1) == f22); + EXPECT_LONGS_EQUAL(3, hybridFactorB.keys().size()); + EXPECT_LONGS_EQUAL(2, hybridFactorB.continuousKeys().size()); + EXPECT_LONGS_EQUAL(1, hybridFactorB.discreteKeys().size()); } /* ************************************************************************* */ -TEST(HybridGaussianFactor, Printing) { +TEST_DISABLED(HybridGaussianFactor, Printing) { using namespace test_constructor; HybridGaussianFactor hybridFactor(m1, {f10, f11}); @@ -138,6 +123,7 @@ Hybrid [x1 x2; 1]{ ] b = [ 0 0 ] No noise model +scalar: 0 1 Leaf : A[x1] = [ @@ -150,6 +136,7 @@ Hybrid [x1 x2; 1]{ ] b = [ 0 0 ] No noise model +scalar: 0 } )"; diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index f30085f020..9fdc1aaea5 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -13,38 +13,35 @@ * @file testHybridGaussianFactorGraph.cpp * @date Mar 11, 2022 * @author Fan Jiang + * @author Varun Agrawal + * @author Frank Dellaert */ -#include -#include +#include #include #include #include #include #include #include -#include #include #include #include #include #include -#include +#include #include #include -#include #include #include #include #include -#include +#include +#include + #include -#include -#include -#include #include -#include #include #include "Switching.h" @@ -53,17 +50,15 @@ using namespace std; using namespace gtsam; -using gtsam::symbol_shorthand::D; using gtsam::symbol_shorthand::M; using gtsam::symbol_shorthand::N; using gtsam::symbol_shorthand::X; -using gtsam::symbol_shorthand::Y; using gtsam::symbol_shorthand::Z; // Set up sampling std::mt19937_64 kRng(42); -static const DiscreteKey m1(M(1), 2); +static const DiscreteKey m0(M(0), 2), m1(M(1), 2), m2(M(2), 2); /* ************************************************************************* */ TEST(HybridGaussianFactorGraph, Creation) { @@ -76,10 +71,10 @@ TEST(HybridGaussianFactorGraph, Creation) { // Define a hybrid gaussian conditional P(x0|x1, c0) // and add it to the factor graph. HybridGaussianConditional gm( - {M(0), 2}, + 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()); @@ -97,22 +92,6 @@ TEST(HybridGaussianFactorGraph, EliminateSequential) { EXPECT_LONGS_EQUAL(result.first->size(), 1); } -/* ************************************************************************* */ -TEST(HybridGaussianFactorGraph, EliminateMultifrontal) { - // Test multifrontal elimination - HybridGaussianFactorGraph hfg; - - // Add priors on x0 and c1 - hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); - hfg.add(DecisionTreeFactor(m1, {2, 8})); - - Ordering ordering; - ordering.push_back(X(0)); - auto result = hfg.eliminatePartialMultifrontal(ordering); - - EXPECT_LONGS_EQUAL(result.first->size(), 1); - EXPECT_LONGS_EQUAL(result.second->size(), 1); -} /* ************************************************************************* */ namespace two { @@ -138,7 +117,9 @@ TEST(HybridGaussianFactorGraph, hybridEliminationOneFactor) { // Check that factor is discrete and correct auto factor = std::dynamic_pointer_cast(result.second); CHECK(factor); - EXPECT(assert_equal(DecisionTreeFactor{m1, "1 1"}, *factor)); + // regression test + EXPECT( + assert_equal(DecisionTreeFactor{m1, "15.74961 15.74961"}, *factor, 1e-5)); } /* ************************************************************************* */ @@ -178,7 +159,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) { // Discrete probability table for c1 hfg.add(DecisionTreeFactor(m1, {2, 8})); // Joint discrete probability table for c1, c2 - hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4")); + hfg.add(DecisionTreeFactor({m1, m2}, "1 2 3 4")); HybridBayesNet::shared_ptr result = hfg.eliminateSequential(); @@ -187,295 +168,233 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) { } /* ************************************************************************* */ -TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) { - HybridGaussianFactorGraph hfg; - - hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); - hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); - - hfg.add(HybridGaussianFactor({M(1), 2}, two::components(X(1)))); - - hfg.add(DecisionTreeFactor(m1, {2, 8})); - // TODO(Varun) Adding extra discrete variable not connected to continuous - // variable throws segfault - // hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4")); - - HybridBayesTree::shared_ptr result = hfg.eliminateMultifrontal(); - - // The bayes tree should have 3 cliques - EXPECT_LONGS_EQUAL(3, result->size()); - // GTSAM_PRINT(*result); - // GTSAM_PRINT(*result->marginalFactor(M(2))); -} - -/* ************************************************************************* */ -TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) { - HybridGaussianFactorGraph hfg; - - // Prior on x0 - hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1)); - // Factor between x0-x1 - hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); - - // Hybrid factor P(x1|c1) - hfg.add(HybridGaussianFactor(m1, two::components(X(1)))); - // Prior factor on c1 - hfg.add(DecisionTreeFactor(m1, {2, 8})); - - // Get a constrained ordering keeping c1 last - auto ordering_full = HybridOrdering(hfg); - - // Returns a Hybrid Bayes Tree with distribution P(x0|x1)P(x1|c1)P(c1) - HybridBayesTree::shared_ptr hbt = hfg.eliminateMultifrontal(ordering_full); - - EXPECT_LONGS_EQUAL(3, hbt->size()); -} - -/* ************************************************************************* */ -/* - * This test is about how to assemble the Bayes Tree roots after we do partial - * elimination - */ -TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) { - HybridGaussianFactorGraph hfg; - - hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); - hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1)); - - hfg.add(HybridGaussianFactor({M(0), 2}, two::components(X(0)))); - hfg.add(HybridGaussianFactor({M(1), 2}, two::components(X(2)))); - - hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4")); - - hfg.add(JacobianFactor(X(3), I_3x3, X(4), -I_3x3, Z_3x1)); - hfg.add(JacobianFactor(X(4), I_3x3, X(5), -I_3x3, Z_3x1)); - - hfg.add(HybridGaussianFactor({M(3), 2}, two::components(X(3)))); - hfg.add(HybridGaussianFactor({M(2), 2}, two::components(X(5)))); - - auto ordering_full = - Ordering::ColamdConstrainedLast(hfg, {M(0), M(1), M(2), M(3)}); - - const auto [hbt, remaining] = hfg.eliminatePartialMultifrontal(ordering_full); - - // 9 cliques in the bayes tree and 0 remaining variables to eliminate. - EXPECT_LONGS_EQUAL(9, hbt->size()); - EXPECT_LONGS_EQUAL(0, remaining->size()); - - /* - (Fan) Explanation: the Junction tree will need to re-eliminate to get to the - marginal on X(1), which is not possible because it involves eliminating - discrete before continuous. The solution to this, however, is in Murphy02. - TLDR is that this is 1. expensive and 2. inexact. nevertheless it is doable. - And I believe that we should do this. - */ -} - -void dotPrint(const HybridGaussianFactorGraph::shared_ptr &hfg, - const HybridBayesTree::shared_ptr &hbt, - const Ordering &ordering) { - DotWriter dw; - dw.positionHints['c'] = 2; - dw.positionHints['x'] = 1; - std::cout << hfg->dot(DefaultKeyFormatter, dw); - std::cout << "\n"; - hbt->dot(std::cout); - - std::cout << "\n"; - std::cout << hfg->eliminateSequential(ordering)->dot(DefaultKeyFormatter, dw); -} - -/* ************************************************************************* */ -// TODO(fan): make a graph like Varun's paper one -TEST(HybridGaussianFactorGraph, Switching) { - auto N = 12; - auto hfg = makeSwitchingChain(N); - - // X(5) will be the center, X(1-4), X(6-9) - // X(3), X(7) - // X(2), X(8) - // X(1), X(4), X(6), X(9) - // M(5) will be the center, M(1-4), M(6-8) - // M(3), M(7) - // M(1), M(4), M(2), M(6), M(8) - // auto ordering_full = - // Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7), - // X(5), - // M(1), M(4), M(2), M(6), M(8), M(3), M(7), M(5)}); - KeyVector ordering; - - { - 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); }); - - auto [ndX, lvls] = makeBinaryOrdering(ordX); - std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering)); - // TODO(dellaert): this has no effect! - for (auto &l : lvls) { - l = -l; - } - } - { - 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::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering)); - const auto [ndC, lvls] = makeBinaryOrdering(ordC); - std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering)); +// Test API for the smallest switching network. +// None of these are regression tests. +TEST(HybridBayesNet, Switching) { + // Create switching network with two continuous variables and one discrete: + // ϕ(x0) ϕ(x0,x1,m0) ϕ(x1;z1) ϕ(m0) + const double betweenSigma = 0.3, priorSigma = 0.1; + Switching s(2, betweenSigma, priorSigma); + + // Check size of linearized factor graph + const HybridGaussianFactorGraph& graph = s.linearizedFactorGraph; + EXPECT_LONGS_EQUAL(4, graph.size()); + + // Create some continuous and discrete values + const VectorValues continuousValues{{X(0), Vector1(0.1)}, + {X(1), Vector1(1.2)}}; + const DiscreteValues modeZero{{M(0), 0}}, modeOne{{M(0), 1}}; + + // Get the hybrid gaussian factor and check it is as expected + auto hgf = std::dynamic_pointer_cast(graph.at(1)); + CHECK(hgf); + + // Get factors and scalars for both modes + auto [factor0, scalar0] = (*hgf)(modeZero); + auto [factor1, scalar1] = (*hgf)(modeOne); + CHECK(factor0); + CHECK(factor1); + + // Check scalars against negLogConstant of noise model + auto betweenModel = noiseModel::Isotropic::Sigma(1, betweenSigma); + EXPECT_DOUBLES_EQUAL(betweenModel->negLogConstant(), scalar0, 1e-9); + EXPECT_DOUBLES_EQUAL(betweenModel->negLogConstant(), scalar1, 1e-9); + + // Check error for M(0) = 0 + const HybridValues values0{continuousValues, modeZero}; + double expectedError0 = 0; + 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); + 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); + + // Check that the actual error tree matches the expected one + EXPECT(assert_equal(expectedErrors, actualErrors, 1e-5)); + + // Check probPrime + const double probPrime0 = graph.probPrime(values0); + EXPECT_DOUBLES_EQUAL(std::exp(-expectedError0), probPrime0, 1e-5); + + const double probPrime1 = graph.probPrime(values1); + EXPECT_DOUBLES_EQUAL(std::exp(-expectedError1), probPrime1, 1e-5); + + // Check discretePosterior + const AlgebraicDecisionTree graphPosterior = + graph.discretePosterior(continuousValues); + const double sum = probPrime0 + probPrime1; + const AlgebraicDecisionTree expectedPosterior( + M(0), probPrime0 / sum, probPrime1 / sum); + EXPECT(assert_equal(expectedPosterior, graphPosterior, 1e-5)); + + // Make the clique of factors connected to x0: + HybridGaussianFactorGraph factors_x0; + factors_x0.push_back(graph.at(0)); + factors_x0.push_back(hgf); + + // Test collectProductFactor + auto productFactor = factors_x0.collectProductFactor(); + + // For M(0) = 0 + auto [gaussianFactor0, actualScalar0] = productFactor(modeZero); + EXPECT(gaussianFactor0.size() == 2); + EXPECT_DOUBLES_EQUAL((*hgf)(modeZero).second, actualScalar0, 1e-5); + + // For M(0) = 1 + auto [gaussianFactor1, actualScalar1] = productFactor(modeOne); + EXPECT(gaussianFactor1.size() == 2); + EXPECT_DOUBLES_EQUAL((*hgf)(modeOne).second, actualScalar1, 1e-5); + + // Test eliminate x0 + const Ordering ordering{X(0)}; + auto [conditional, factor] = factors_x0.eliminate(ordering); + + // Check the conditional + CHECK(conditional); + EXPECT(conditional->isHybrid()); + auto p_x0_given_x1_m = conditional->asHybrid(); + CHECK(p_x0_given_x1_m); + EXPECT(HybridGaussianConditional::CheckInvariants(*p_x0_given_x1_m, values1)); + EXPECT_LONGS_EQUAL(1, p_x0_given_x1_m->nrFrontals()); // x0 + EXPECT_LONGS_EQUAL(2, p_x0_given_x1_m->nrParents()); // x1, m0 + + // Check the remaining factor + EXPECT(factor); + EXPECT(std::dynamic_pointer_cast(factor)); + auto phi_x1_m = std::dynamic_pointer_cast(factor); + EXPECT_LONGS_EQUAL(2, phi_x1_m->keys().size()); // x1, m0 + // 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); + EXPECT_DOUBLES_EQUAL(scalar1 - (*p_x0_given_x1_m)(modeOne)->negLogConstant(), + (*phi_x1_m)(modeOne).second, + 1e-9); + + // Check that the conditional and remaining factor are consistent for both + // modes + for (auto&& mode : {modeZero, modeOne}) { + const auto gc = (*p_x0_given_x1_m)(mode); + const auto [gf, scalar] = (*phi_x1_m)(mode); + + // The error of the original factors should equal the sum of errors of the + // conditional and remaining factor, modulo the normalization constant of + // the conditional. + double originalError = factors_x0.error({continuousValues, mode}); + const double actualError = gc->negLogConstant() + + gc->error(continuousValues) + + gf->error(continuousValues) + scalar; + EXPECT_DOUBLES_EQUAL(originalError, actualError, 1e-9); } - auto ordering_full = Ordering(ordering); - - // GTSAM_PRINT(*hfg); - // GTSAM_PRINT(ordering_full); - - const auto [hbt, remaining] = - hfg->eliminatePartialMultifrontal(ordering_full); - - // 12 cliques in the bayes tree and 0 remaining variables to eliminate. - EXPECT_LONGS_EQUAL(12, hbt->size()); - EXPECT_LONGS_EQUAL(0, remaining->size()); -} -/* ************************************************************************* */ -// TODO(fan): make a graph like Varun's paper one -TEST(HybridGaussianFactorGraph, SwitchingISAM) { - auto N = 11; - auto hfg = makeSwitchingChain(N); - - // X(5) will be the center, X(1-4), X(6-9) - // X(3), X(7) - // X(2), X(8) - // X(1), X(4), X(6), X(9) - // M(5) will be the center, M(1-4), M(6-8) - // M(3), M(7) - // M(1), M(4), M(2), M(6), M(8) - // auto ordering_full = - // Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7), - // X(5), - // M(1), M(4), M(2), M(6), M(8), M(3), M(7), M(5)}); - KeyVector ordering; - - { - 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); }); - - auto [ndX, lvls] = makeBinaryOrdering(ordX); - std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering)); - // TODO(dellaert): this has no effect! - for (auto &l : lvls) { - l = -l; - } - } - { - 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::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering)); - const auto [ndC, lvls] = makeBinaryOrdering(ordC); - std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering)); + // Create a clique for x1 + HybridGaussianFactorGraph factors_x1; + factors_x1.push_back( + factor); // Use the remaining factor from previous elimination + factors_x1.push_back( + graph.at(2)); // Add the factor for x1 from the original graph + + // Test collectProductFactor for x1 clique + auto productFactor_x1 = factors_x1.collectProductFactor(); + + // For M(0) = 0 + auto [gaussianFactor_x1_0, actualScalar_x1_0] = productFactor_x1(modeZero); + EXPECT_LONGS_EQUAL(2, gaussianFactor_x1_0.size()); + // NOTE(Frank): prior on x1 does not contribute to the scalar + EXPECT_DOUBLES_EQUAL((*phi_x1_m)(modeZero).second, actualScalar_x1_0, 1e-5); + + // For M(0) = 1 + auto [gaussianFactor_x1_1, actualScalar_x1_1] = productFactor_x1(modeOne); + EXPECT_LONGS_EQUAL(2, gaussianFactor_x1_1.size()); + // NOTE(Frank): prior on x1 does not contribute to the scalar + EXPECT_DOUBLES_EQUAL((*phi_x1_m)(modeOne).second, actualScalar_x1_1, 1e-5); + + // Test eliminate for x1 clique + Ordering ordering_x1{X(1)}; + auto [conditional_x1, factor_x1] = factors_x1.eliminate(ordering_x1); + + // Check the conditional for x1 + CHECK(conditional_x1); + EXPECT(conditional_x1->isHybrid()); + auto p_x1_given_m = conditional_x1->asHybrid(); + CHECK(p_x1_given_m); + EXPECT_LONGS_EQUAL(1, p_x1_given_m->nrFrontals()); // x1 + EXPECT_LONGS_EQUAL(1, p_x1_given_m->nrParents()); // m0 + + // Check the remaining factor for x1 + CHECK(factor_x1); + auto phi_x1 = std::dynamic_pointer_cast(factor_x1); + CHECK(phi_x1); + EXPECT_LONGS_EQUAL(1, phi_x1->keys().size()); // m0 + // We can't really check the error of the decision tree factor phi_x1, because + // the continuos factor whose error(kEmpty) we need is not available.. + + // 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}) { + auto gc_x1 = (*p_x1_given_m)(mode); + double originalError_x1 = factors_x1.error({continuousValues, mode}); + const double actualError = gc_x1->negLogConstant() + + gc_x1->error(continuousValues) + + phi_x1->error(mode); + EXPECT_DOUBLES_EQUAL(originalError_x1, actualError, 1e-9); } - auto ordering_full = Ordering(ordering); - - const auto [hbt, remaining] = - hfg->eliminatePartialMultifrontal(ordering_full); - - auto new_fg = makeSwitchingChain(12); - auto isam = HybridGaussianISAM(*hbt); - // Run an ISAM update. - HybridGaussianFactorGraph factorGraph; - factorGraph.push_back(new_fg->at(new_fg->size() - 2)); - factorGraph.push_back(new_fg->at(new_fg->size() - 1)); - isam.update(factorGraph); + // Now test full elimination of the graph: + auto hybridBayesNet = graph.eliminateSequential(); + CHECK(hybridBayesNet); - // ISAM should have 12 factors after the last update - EXPECT_LONGS_EQUAL(12, isam.size()); + // Check that the posterior P(M|X=continuousValues) from the Bayes net is the + // same as the same posterior from the graph. This is a sanity check that the + // elimination is done correctly. + AlgebraicDecisionTree bnPosterior = + hybridBayesNet->discretePosterior(continuousValues); + EXPECT(assert_equal(graphPosterior, bnPosterior)); } -/* ************************************************************************* */ -TEST(HybridGaussianFactorGraph, SwitchingTwoVar) { - const int N = 7; - auto hfg = makeSwitchingChain(N, X); - hfg->push_back(*makeSwitchingChain(N, Y, D)); - - for (int t = 1; t <= N; t++) { - hfg->add(JacobianFactor(X(t), I_3x3, Y(t), -I_3x3, Vector3(1.0, 0.0, 0.0))); - } - - KeyVector ordering; +/* ****************************************************************************/ +// Test subset of API for switching network with 3 states. +// None of these are regression tests. +TEST(HybridGaussianFactorGraph, ErrorAndProbPrime) { + // Create switching network with three continuous variables and two discrete: + // ϕ(x0) ϕ(x0,x1,m0) ϕ(x1,x2,m1) ϕ(x1;z1) ϕ(x2;z2) ϕ(m0) ϕ(m0,m1) + Switching s(3); - KeyVector naturalX(N); - std::iota(naturalX.begin(), naturalX.end(), 1); - KeyVector ordX; - for (size_t i = 1; i <= N; i++) { - ordX.emplace_back(X(i)); - ordX.emplace_back(Y(i)); - } + // Check size of linearized factor graph + const HybridGaussianFactorGraph& graph = s.linearizedFactorGraph; + EXPECT_LONGS_EQUAL(7, graph.size()); - for (size_t i = 1; i <= N - 1; i++) { - ordX.emplace_back(M(i)); - } - for (size_t i = 1; i <= N - 1; i++) { - ordX.emplace_back(D(i)); - } + // Eliminate the graph + const HybridBayesNet::shared_ptr hybridBayesNet = graph.eliminateSequential(); - { - DotWriter dw; - dw.positionHints['x'] = 1; - dw.positionHints['c'] = 0; - dw.positionHints['d'] = 3; - dw.positionHints['y'] = 2; - // std::cout << hfg->dot(DefaultKeyFormatter, dw); - // std::cout << "\n"; - } + const HybridValues delta = hybridBayesNet->optimize(); + const double error = graph.error(delta); - { - DotWriter dw; - dw.positionHints['y'] = 9; - // dw.positionHints['c'] = 0; - // dw.positionHints['d'] = 3; - dw.positionHints['x'] = 1; - // std::cout << "\n"; - // std::cout << hfg->eliminateSequential(Ordering(ordX)) - // ->dot(DefaultKeyFormatter, dw); - // hfg->eliminateMultifrontal(Ordering(ordX))->dot(std::cout); - } + // Check that the probability prime is the exponential of the error + EXPECT(assert_equal(graph.probPrime(delta), exp(-error), 1e-7)); - Ordering ordering_partial; - for (size_t i = 1; i <= N; i++) { - ordering_partial.emplace_back(X(i)); - ordering_partial.emplace_back(Y(i)); - } - const auto [hbn, remaining] = - hfg->eliminatePartialSequential(ordering_partial); - - EXPECT_LONGS_EQUAL(14, hbn->size()); - EXPECT_LONGS_EQUAL(11, remaining->size()); - - { - DotWriter dw; - dw.positionHints['x'] = 1; - dw.positionHints['c'] = 0; - dw.positionHints['d'] = 3; - dw.positionHints['y'] = 2; - // std::cout << remaining->dot(DefaultKeyFormatter, dw); - // std::cout << "\n"; - } + // Check that the posterior P(M|X=continuousValues) from the Bayes net is the + // same as the same posterior from the graph. This is a sanity check that the + // elimination is done correctly. + const AlgebraicDecisionTree graphPosterior = + graph.discretePosterior(delta.continuous()); + const AlgebraicDecisionTree bnPosterior = + hybridBayesNet->discretePosterior(delta.continuous()); + EXPECT(assert_equal(graphPosterior, bnPosterior)); } -/* ****************************************************************************/ +/* ************************************************************************* */ // Select a particular continuous factor graph given a discrete assignment TEST(HybridGaussianFactorGraph, DiscreteSelection) { Switching s(3); @@ -546,23 +465,43 @@ TEST(HybridGaussianFactorGraph, optimize) { // Test adding of gaussian conditional and re-elimination. TEST(HybridGaussianFactorGraph, Conditionals) { Switching switching(4); - HybridGaussianFactorGraph hfg; - hfg.push_back(switching.linearizedFactorGraph.at(0)); // P(X1) + HybridGaussianFactorGraph hfg; + hfg.push_back(switching.linearizedFactorGraph.at(0)); // P(X0) Ordering ordering; ordering.push_back(X(0)); HybridBayesNet::shared_ptr bayes_net = hfg.eliminateSequential(ordering); - hfg.push_back(switching.linearizedFactorGraph.at(1)); // P(X1, X2 | M1) - hfg.push_back(*bayes_net); - hfg.push_back(switching.linearizedFactorGraph.at(2)); // P(X2, X3 | M2) - hfg.push_back(switching.linearizedFactorGraph.at(5)); // P(M1) - ordering.push_back(X(1)); - ordering.push_back(X(2)); - ordering.push_back(M(0)); - ordering.push_back(M(1)); + HybridGaussianFactorGraph hfg2; + hfg2.push_back(*bayes_net); // P(X0) + hfg2.push_back(switching.linearizedFactorGraph.at(1)); // P(X0, X1 | M0) + hfg2.push_back(switching.linearizedFactorGraph.at(2)); // P(X1, X2 | M1) + hfg2.push_back(switching.linearizedFactorGraph.at(5)); // P(M1) + ordering += X(1), X(2), M(0), M(1); + + // Created product of first two factors and check eliminate: + HybridGaussianFactorGraph fragment; + fragment.push_back(hfg2[0]); + fragment.push_back(hfg2[1]); + + // Check that product + HybridGaussianProductFactor product = fragment.collectProductFactor(); + auto leaf = fragment(DiscreteValues{{M(0), 0}}); + EXPECT_LONGS_EQUAL(2, leaf.size()); + + // Check product and that pruneEmpty does not touch it + auto pruned = product.removeEmpty(); + LONGS_EQUAL(2, pruned.nrLeaves()); + + // Test eliminate + auto [hybridConditional, factor] = fragment.eliminate({X(0)}); + EXPECT(hybridConditional->isHybrid()); + EXPECT(hybridConditional->keys() == KeyVector({X(0), X(1), M(0)})); + + EXPECT(dynamic_pointer_cast(factor)); + EXPECT(factor->keys() == KeyVector({X(1), M(0)})); - bayes_net = hfg.eliminateSequential(ordering); + bayes_net = hfg2.eliminateSequential(ordering); HybridValues result = bayes_net->optimize(); @@ -581,51 +520,6 @@ TEST(HybridGaussianFactorGraph, Conditionals) { EXPECT(assert_equal(expected_discrete, result.discrete())); } -/* ****************************************************************************/ -// Test hybrid gaussian factor graph error and unnormalized probabilities -TEST(HybridGaussianFactorGraph, ErrorAndProbPrime) { - Switching s(3); - - HybridGaussianFactorGraph graph = s.linearizedFactorGraph; - - HybridBayesNet::shared_ptr hybridBayesNet = graph.eliminateSequential(); - - const HybridValues delta = hybridBayesNet->optimize(); - const double error = graph.error(delta); - - // regression - EXPECT(assert_equal(1.58886, error, 1e-5)); - - // Real test: - EXPECT(assert_equal(graph.probPrime(delta), exp(-error), 1e-7)); -} - -/* ****************************************************************************/ -// Test hybrid gaussian factor graph error and unnormalized probabilities -TEST(HybridGaussianFactorGraph, ErrorAndProbPrimeTree) { - // Create switching network with three continuous variables and two discrete: - // ϕ(x0) ϕ(x0,x1,m0) ϕ(x1,x2,m1) ϕ(x0;z0) ϕ(x1;z1) ϕ(x2;z2) ϕ(m0) ϕ(m0,m1) - Switching s(3); - - const HybridGaussianFactorGraph &graph = s.linearizedFactorGraph; - - const HybridBayesNet::shared_ptr hybridBayesNet = graph.eliminateSequential(); - - const HybridValues delta = hybridBayesNet->optimize(); - - // regression test for errorTree - std::vector leaves = {2.7916153, 1.5888555, 1.7233422, 1.6191947}; - AlgebraicDecisionTree expectedErrors(s.modes, leaves); - const auto error_tree = graph.errorTree(delta.continuous()); - EXPECT(assert_equal(expectedErrors, error_tree, 1e-7)); - - // regression test for discretePosterior - const AlgebraicDecisionTree expectedPosterior( - s.modes, std::vector{0.095516068, 0.31800092, 0.27798511, 0.3084979}); - auto posterior = graph.discretePosterior(delta.continuous()); - EXPECT(assert_equal(expectedPosterior, posterior, 1e-7)); -} - /* ****************************************************************************/ // Test hybrid gaussian factor graph errorTree during incremental operation TEST(HybridGaussianFactorGraph, IncrementalErrorTree) { @@ -643,15 +537,13 @@ TEST(HybridGaussianFactorGraph, IncrementalErrorTree) { HybridBayesNet::shared_ptr hybridBayesNet = graph.eliminateSequential(); EXPECT_LONGS_EQUAL(5, hybridBayesNet->size()); + // Check discrete posterior at optimum HybridValues delta = hybridBayesNet->optimize(); - auto error_tree = graph.errorTree(delta.continuous()); - - std::vector discrete_keys = {{M(0), 2}, {M(1), 2}}; - std::vector leaves = {2.7916153, 1.5888555, 1.7233422, 1.6191947}; - AlgebraicDecisionTree expected_error(discrete_keys, leaves); - - // regression - EXPECT(assert_equal(expected_error, error_tree, 1e-7)); + AlgebraicDecisionTree graphPosterior = + graph.discretePosterior(delta.continuous()); + AlgebraicDecisionTree bnPosterior = + hybridBayesNet->discretePosterior(delta.continuous()); + EXPECT(assert_equal(graphPosterior, bnPosterior)); graph = HybridGaussianFactorGraph(); graph.push_back(*hybridBayesNet); @@ -662,26 +554,21 @@ TEST(HybridGaussianFactorGraph, IncrementalErrorTree) { EXPECT_LONGS_EQUAL(7, hybridBayesNet->size()); delta = hybridBayesNet->optimize(); - auto error_tree2 = graph.errorTree(delta.continuous()); - - // regression - leaves = {0.50985198, 0.0097577296, 0.50009425, 0, - 0.52922138, 0.029127133, 0.50985105, 0.0097567964}; - AlgebraicDecisionTree expected_error2(s.modes, leaves); - EXPECT(assert_equal(expected_error, error_tree, 1e-7)); + graphPosterior = graph.discretePosterior(delta.continuous()); + bnPosterior = hybridBayesNet->discretePosterior(delta.continuous()); + EXPECT(assert_equal(graphPosterior, bnPosterior)); } /* ****************************************************************************/ -// Check that assembleGraphTree assembles Gaussian factor graphs for each -// assignment. -TEST(HybridGaussianFactorGraph, assembleGraphTree) { +// Check that collectProductFactor works correctly. +TEST(HybridGaussianFactorGraph, collectProductFactor) { const int num_measurements = 1; - auto fg = tiny::createHybridGaussianFactorGraph( - num_measurements, VectorValues{{Z(0), Vector1(5.0)}}); + VectorValues vv{{Z(0), Vector1(5.0)}}; + auto fg = tiny::createHybridGaussianFactorGraph(num_measurements, vv); EXPECT_LONGS_EQUAL(3, fg.size()); // Assemble graph tree: - auto actual = fg.assembleGraphTree(); + auto actual = fg.collectProductFactor(); // Create expected decision tree with two factor graphs: @@ -700,13 +587,15 @@ TEST(HybridGaussianFactorGraph, assembleGraphTree) { DiscreteValues d0{{M(0), 0}}, d1{{M(0), 1}}; // Expected decision tree with two factor graphs: - // f(x0;mode=0)P(x0) and f(x0;mode=1)P(x0) - GaussianFactorGraphTree expected{ - M(0), GaussianFactorGraph(std::vector{(*hybrid)(d0), prior}), - GaussianFactorGraph(std::vector{(*hybrid)(d1), prior})}; - - EXPECT(assert_equal(expected(d0), actual(d0), 1e-5)); - EXPECT(assert_equal(expected(d1), actual(d1), 1e-5)); + // f(x0;mode=0)P(x0) + GaussianFactorGraph expectedFG0{(*hybrid)(d0).first, prior}; + EXPECT(assert_equal(expectedFG0, actual(d0).first, 1e-5)); + EXPECT(assert_equal(0.0, actual(d0).second, 1e-5)); + + // f(x0;mode=1)P(x0) + GaussianFactorGraph expectedFG1{(*hybrid)(d1).first, prior}; + EXPECT(assert_equal(expectedFG1, actual(d1).first, 1e-5)); + EXPECT(assert_equal(1.79176, actual(d1).second, 1e-5)); } /* ****************************************************************************/ @@ -746,7 +635,6 @@ bool ratioTest(const HybridBayesNet &bn, const VectorValues &measurements, // Test ratios for a number of independent samples: for (size_t i = 0; i < num_samples; i++) { HybridValues sample = bn.sample(&kRng); - // GTSAM_PRINT(sample); // std::cout << "ratio: " << compute_ratio(&sample) << std::endl; if (std::abs(expected_ratio - compute_ratio(&sample)) > 1e-6) return false; } diff --git a/gtsam/hybrid/tests/testHybridGaussianProductFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianProductFactor.cpp new file mode 100644 index 0000000000..f41c5f0aa2 --- /dev/null +++ b/gtsam/hybrid/tests/testHybridGaussianProductFactor.cpp @@ -0,0 +1,196 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testHybridGaussianProductFactor.cpp + * @brief Unit tests for HybridGaussianProductFactor + * @author Frank Dellaert + * @date October 2024 + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +// Include for test suite +#include + +#include + +using namespace std; +using namespace gtsam; +using symbol_shorthand::M; +using symbol_shorthand::X; + +/* ************************************************************************* */ +namespace examples { +static const DiscreteKey m1(M(1), 2), m2(M(2), 3); + +const auto A1 = Matrix::Zero(2, 1); +const auto A2 = Matrix::Zero(2, 2); +const auto b = Matrix::Zero(2, 1); + +const auto f10 = std::make_shared(X(1), A1, X(2), A2, b); +const auto f11 = std::make_shared(X(1), A1, X(2), A2, b); +const HybridGaussianFactor hybridFactorA(m1, {{f10, 10}, {f11, 11}}); + +const auto A3 = Matrix::Zero(2, 3); +const auto f20 = std::make_shared(X(1), A1, X(3), A3, b); +const auto f21 = std::make_shared(X(1), A1, X(3), A3, b); +const auto f22 = std::make_shared(X(1), A1, X(3), A3, b); + +const HybridGaussianFactor hybridFactorB(m2, {{f20, 20}, {f21, 21}, {f22, 22}}); +// Simulate a pruned hybrid factor, in this case m2==1 is nulled out. +const HybridGaussianFactor prunedFactorB( + m2, {{f20, 20}, {nullptr, 1000}, {f22, 22}}); +} // namespace examples + +/* ************************************************************************* */ +// Constructor +TEST(HybridGaussianProductFactor, Construct) { + HybridGaussianProductFactor product; +} + +/* ************************************************************************* */ +// Add two Gaussian factors and check only one leaf in tree +TEST(HybridGaussianProductFactor, AddTwoGaussianFactors) { + using namespace examples; + + HybridGaussianProductFactor product; + product += f10; + product += f11; + + // Check that the product has only one leaf and no discrete variables. + EXPECT_LONGS_EQUAL(1, product.nrLeaves()); + EXPECT(product.labels().empty()); + + // Retrieve the single leaf + auto leaf = product(Assignment()); + + // Check that the leaf contains both factors + EXPECT_LONGS_EQUAL(2, leaf.first.size()); + EXPECT(leaf.first.at(0) == f10); + EXPECT(leaf.first.at(1) == f11); + EXPECT_DOUBLES_EQUAL(0, leaf.second, 1e-9); +} + +/* ************************************************************************* */ +// Add two GaussianConditionals and check the resulting tree +TEST(HybridGaussianProductFactor, AddTwoGaussianConditionals) { + // Create two GaussianConditionals + Vector1 d(1.0); + Matrix11 R = I_1x1, S = I_1x1; + auto gc1 = std::make_shared(X(1), d, R, X(2), S); + auto gc2 = std::make_shared(X(2), d, R); + + // Create a HybridGaussianProductFactor and add the conditionals + HybridGaussianProductFactor product; + product += std::static_pointer_cast(gc1); + product += std::static_pointer_cast(gc2); + + // Check that the product has only one leaf and no discrete variables + EXPECT_LONGS_EQUAL(1, product.nrLeaves()); + EXPECT(product.labels().empty()); + + // Retrieve the single leaf + auto leaf = product(Assignment()); + + // Check that the leaf contains both conditionals + EXPECT_LONGS_EQUAL(2, leaf.first.size()); + EXPECT(leaf.first.at(0) == gc1); + EXPECT(leaf.first.at(1) == gc2); + EXPECT_DOUBLES_EQUAL(0, leaf.second, 1e-9); +} + +/* ************************************************************************* */ +// Check AsProductFactor +TEST(HybridGaussianProductFactor, AsProductFactor) { + using namespace examples; + auto product = hybridFactorA.asProductFactor(); + + // Let's check that this worked: + Assignment mode; + mode[m1.first] = 0; + auto actual = product(mode); + EXPECT(actual.first.at(0) == f10); + EXPECT_DOUBLES_EQUAL(10, actual.second, 1e-9); + + // TODO(Frank): when killed hiding, f11 should also be there +} + +/* ************************************************************************* */ +// "Add" one hybrid factors together. +TEST(HybridGaussianProductFactor, AddOne) { + using namespace examples; + HybridGaussianProductFactor product; + product += hybridFactorA; + + // Let's check that this worked: + Assignment mode; + mode[m1.first] = 0; + auto actual = product(mode); + EXPECT(actual.first.at(0) == f10); + EXPECT_DOUBLES_EQUAL(10, actual.second, 1e-9); + + // TODO(Frank): when killed hiding, f11 should also be there +} + +/* ************************************************************************* */ +// "Add" two HFG together. +TEST(HybridGaussianProductFactor, AddTwo) { + using namespace examples; + + // Create product of two hybrid factors: it will be a decision tree now on + // both discrete variables m1 and m2: + HybridGaussianProductFactor product; + product += hybridFactorA; + product += hybridFactorB; + + // Let's check that this worked: + auto actual00 = product({{M(1), 0}, {M(2), 0}}); + EXPECT(actual00.first.at(0) == f10); + EXPECT(actual00.first.at(1) == f20); + EXPECT_DOUBLES_EQUAL(10 + 20, actual00.second, 1e-9); + + auto actual12 = product({{M(1), 1}, {M(2), 2}}); + // TODO(Frank): when killed hiding, these should also equal: + // EXPECT(actual12.first.at(0) == f11); + // EXPECT(actual12.first.at(1) == f22); + EXPECT_DOUBLES_EQUAL(11 + 22, actual12.second, 1e-9); +} + +/* ************************************************************************* */ +// "Add" two HFG together. +TEST(HybridGaussianProductFactor, AddPruned) { + using namespace examples; + + // Create product of two hybrid factors: it will be a decision tree now on + // both discrete variables m1 and m2: + HybridGaussianProductFactor product; + product += hybridFactorA; + product += prunedFactorB; + EXPECT_LONGS_EQUAL(6, product.nrLeaves()); + + auto pruned = product.removeEmpty(); + EXPECT_LONGS_EQUAL(5, pruned.nrLeaves()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/hybrid/tests/testHybridMotionModel.cpp b/gtsam/hybrid/tests/testHybridMotionModel.cpp index cbfdc7fe4e..16a2bd476d 100644 --- a/gtsam/hybrid/tests/testHybridMotionModel.cpp +++ b/gtsam/hybrid/tests/testHybridMotionModel.cpp @@ -192,24 +192,29 @@ TEST(HybridGaussianFactor, TwoStateModel2) { HybridBayesNet hbn = CreateBayesNet(hybridMotionModel); HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); - // Check that ratio of Bayes net and factor graph for different modes is - // equal for several values of {x0,x1}. + HybridBayesNet::shared_ptr eliminated = gfg.eliminateSequential(); + for (VectorValues vv : {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}}, VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) { vv.insert(given); // add measurements for HBN - HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}}); - EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0), - gfg.error(hv1) / hbn.error(hv1), 1e-9); - } + const auto& expectedDiscretePosterior = hbn.discretePosterior(vv); - HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + // Equality of posteriors asserts that the factor graph is correct (same + // ratios for all modes) + EXPECT( + assert_equal(expectedDiscretePosterior, gfg.discretePosterior(vv))); + + // This one asserts that HBN resulting from elimination is correct. + EXPECT(assert_equal(expectedDiscretePosterior, + eliminated->discretePosterior(vv))); + } // Importance sampling run with 100k samples gives 50.095/49.905 // approximateDiscreteMarginal(hbn, hybridMotionModel, given); // Since no measurement on x1, we a 50/50 probability - auto p_m = bn->at(2)->asDiscrete(); + auto p_m = eliminated->at(2)->asDiscrete(); EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()({{M(1), 0}}), 1e-9); EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()({{M(1), 1}}), 1e-9); } @@ -221,6 +226,7 @@ TEST(HybridGaussianFactor, TwoStateModel2) { HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true); HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr eliminated = gfg.eliminateSequential(); // Check that ratio of Bayes net and factor graph for different modes is // equal for several values of {x0,x1}. @@ -228,17 +234,22 @@ TEST(HybridGaussianFactor, TwoStateModel2) { {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}}, VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) { vv.insert(given); // add measurements for HBN - HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}}); - EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0), - gfg.error(hv1) / hbn.error(hv1), 1e-9); - } + const auto& expectedDiscretePosterior = hbn.discretePosterior(vv); - HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + // Equality of posteriors asserts that the factor graph is correct (same + // ratios for all modes) + EXPECT( + assert_equal(expectedDiscretePosterior, gfg.discretePosterior(vv))); + + // This one asserts that HBN resulting from elimination is correct. + EXPECT(assert_equal(expectedDiscretePosterior, + eliminated->discretePosterior(vv))); + } // Values taken from an importance sampling run with 100k samples: // approximateDiscreteMarginal(hbn, hybridMotionModel, given); DiscreteConditional expected(m1, "48.3158/51.6842"); - EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002)); + EXPECT(assert_equal(expected, *(eliminated->at(2)->asDiscrete()), 0.002)); } { diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 2b5b267d01..b5486c6cdb 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -529,7 +529,7 @@ TEST(HybridNonlinearFactorGraph, Full_Elimination) { /**************************************************************************** * Test printing */ -TEST(HybridNonlinearFactorGraph, Printing) { +TEST_DISABLED(HybridNonlinearFactorGraph, Printing) { Switching self(3); auto linearizedFactorGraph = self.linearizedFactorGraph; @@ -545,78 +545,98 @@ TEST(HybridNonlinearFactorGraph, Printing) { #ifdef GTSAM_DT_MERGING string expected_hybridFactorGraph = R"( size: 7 -factor 0: +Factor 0 +GaussianFactor: + A[x0] = [ - 10 + 10 ] b = [ -10 ] No noise model -factor 1: -HybridGaussianFactor + +Factor 1 +HybridGaussianFactor: Hybrid [x0 x1; m0]{ Choice(m0) 0 Leaf : A[x0] = [ - -1 + -1 ] A[x1] = [ - 1 + 1 ] b = [ -1 ] No noise model +scalar: 0 1 Leaf : A[x0] = [ - -1 + -1 ] A[x1] = [ - 1 + 1 ] b = [ -0 ] No noise model +scalar: 0 } -factor 2: -HybridGaussianFactor + +Factor 2 +HybridGaussianFactor: Hybrid [x1 x2; m1]{ Choice(m1) 0 Leaf : A[x1] = [ - -1 + -1 ] A[x2] = [ - 1 + 1 ] b = [ -1 ] No noise model +scalar: 0 1 Leaf : A[x1] = [ - -1 + -1 ] A[x2] = [ - 1 + 1 ] b = [ -0 ] No noise model +scalar: 0 } -factor 3: + +Factor 3 +GaussianFactor: + A[x1] = [ - 10 + 10 ] b = [ -10 ] No noise model -factor 4: + +Factor 4 +GaussianFactor: + A[x2] = [ - 10 + 10 ] b = [ -10 ] No noise model -factor 5: P( m0 ): + +Factor 5 +DiscreteFactor: + P( m0 ): Leaf 0.5 -factor 6: P( m1 | m0 ): + +Factor 6 +DiscreteFactor: + P( m1 | m0 ): Choice(m1) 0 Choice(m0) 0 0 Leaf 0.33333333 @@ -625,6 +645,7 @@ factor 6: P( m1 | m0 ): 1 0 Leaf 0.66666667 1 1 Leaf 0.4 + )"; #else string expected_hybridFactorGraph = R"( @@ -717,7 +738,7 @@ factor 6: P( m1 | m0 ): // Expected output for hybridBayesNet. string expected_hybridBayesNet = R"( size: 3 -conditional 0: Hybrid P( x0 | x1 m0) +conditional 0: P( x0 | x1 m0) Discrete Keys = (m0, 2), logNormalizationConstant: 1.38862 @@ -736,7 +757,7 @@ conditional 0: Hybrid P( x0 | x1 m0) logNormalizationConstant: 1.38862 No noise model -conditional 1: Hybrid P( x1 | x2 m0 m1) +conditional 1: P( x1 | x2 m0 m1) Discrete Keys = (m0, 2), (m1, 2), logNormalizationConstant: 1.3935 @@ -771,7 +792,7 @@ conditional 1: Hybrid P( x1 | x2 m0 m1) logNormalizationConstant: 1.3935 No noise model -conditional 2: Hybrid P( x2 | m0 m1) +conditional 2: P( x2 | m0 m1) Discrete Keys = (m0, 2), (m1, 2), logNormalizationConstant: 1.38857 diff --git a/gtsam/hybrid/tests/testSerializationHybrid.cpp b/gtsam/hybrid/tests/testSerializationHybrid.cpp index e09669117e..8258d8615e 100644 --- a/gtsam/hybrid/tests/testSerializationHybrid.cpp +++ b/gtsam/hybrid/tests/testSerializationHybrid.cpp @@ -52,11 +52,11 @@ BOOST_CLASS_EXPORT_GUID(ADT::Leaf, "gtsam_AlgebraicDecisionTree_Leaf"); BOOST_CLASS_EXPORT_GUID(ADT::Choice, "gtsam_AlgebraicDecisionTree_Choice") BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor, "gtsam_HybridGaussianFactor"); -BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::Factors, +BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::FactorValuePairs, "gtsam_HybridGaussianFactor_Factors"); -BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::Factors::Leaf, +BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::FactorValuePairs::Leaf, "gtsam_HybridGaussianFactor_Factors_Leaf"); -BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::Factors::Choice, +BOOST_CLASS_EXPORT_GUID(HybridGaussianFactor::FactorValuePairs::Choice, "gtsam_HybridGaussianFactor_Factors_Choice"); BOOST_CLASS_EXPORT_GUID(HybridGaussianConditional,