From f8d75abfebf40978ea97e9886c6b4d2c1463a614 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 2 Jan 2023 13:11:22 -0500 Subject: [PATCH] name change of Sum to GaussianFactorGraphTree and SumFrontals to assembleGraphTree --- gtsam/hybrid/GaussianMixture.cpp | 16 ++--- gtsam/hybrid/GaussianMixture.h | 19 +++-- gtsam/hybrid/GaussianMixtureFactor.cpp | 12 ++-- gtsam/hybrid/GaussianMixtureFactor.h | 41 ++--------- gtsam/hybrid/HybridBayesNet.cpp | 13 ++++ gtsam/hybrid/HybridFactor.h | 32 +++++++++ gtsam/hybrid/HybridGaussianFactorGraph.cpp | 70 +++++++++---------- gtsam/hybrid/HybridGaussianFactorGraph.h | 2 +- .../tests/testGaussianMixtureFactor.cpp | 2 +- .../tests/testHybridGaussianFactorGraph.cpp | 9 +-- 10 files changed, 118 insertions(+), 98 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 8b8c62399..ad636a5d4 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -51,28 +51,28 @@ GaussianMixture::GaussianMixture( Conditionals(discreteParents, conditionalsList)) {} /* *******************************************************************************/ -GaussianMixture::Sum GaussianMixture::add( - const GaussianMixture::Sum &sum) const { - using Y = GaussianMixtureFactor::GraphAndConstant; +GaussianFactorGraphTree GaussianMixture::add( + const GaussianFactorGraphTree &sum) const { + using Y = GraphAndConstant; auto add = [](const Y &graph1, const Y &graph2) { auto result = graph1.graph; result.push_back(graph2.graph); return Y(result, graph1.constant + graph2.constant); }; - const Sum tree = asGaussianFactorGraphTree(); + const auto tree = asGaussianFactorGraphTree(); return sum.empty() ? tree : sum.apply(tree, add); } /* *******************************************************************************/ -GaussianMixture::Sum GaussianMixture::asGaussianFactorGraphTree() const { +GaussianFactorGraphTree GaussianMixture::asGaussianFactorGraphTree() const { auto lambda = [](const GaussianConditional::shared_ptr &conditional) { GaussianFactorGraph result; result.push_back(conditional); if (conditional) { - return GaussianMixtureFactor::GraphAndConstant( + return GraphAndConstant( result, conditional->logNormalizationConstant()); } else { - return GaussianMixtureFactor::GraphAndConstant(result, 0.0); + return GraphAndConstant(result, 0.0); } }; return {conditionals_, lambda}; @@ -108,7 +108,7 @@ bool GaussianMixture::equals(const HybridFactor &lf, double tol) const { // This will return false if either conditionals_ is empty or e->conditionals_ // is empty, but not if both are empty or both are not empty: if (conditionals_.empty() ^ e->conditionals_.empty()) return false; -std::cout << "checking" << std::endl; + std::cout << "checking" << std::endl; // Check the base and the factors: return BaseFactor::equals(*e, tol) && conditionals_.equals(e->conditionals_, diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index e8ba78d61..240f79dcd 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -59,9 +59,6 @@ class GTSAM_EXPORT GaussianMixture using BaseFactor = HybridFactor; using BaseConditional = Conditional; - /// Alias for DecisionTree of GaussianFactorGraphs - using Sum = DecisionTree; - /// typedef for Decision Tree of Gaussian Conditionals using Conditionals = DecisionTree; @@ -71,7 +68,7 @@ class GTSAM_EXPORT GaussianMixture /** * @brief Convert a DecisionTree of factors into a DT of Gaussian FGs. */ - Sum asGaussianFactorGraphTree() const; + GaussianFactorGraphTree asGaussianFactorGraphTree() const; /** * @brief Helper function to get the pruner functor. @@ -172,6 +169,16 @@ class GTSAM_EXPORT GaussianMixture */ double error(const HybridValues &values) const override; + // /// Calculate probability density for given values `x`. + // double evaluate(const HybridValues &values) const; + + // /// Evaluate probability density, sugar. + // double operator()(const HybridValues &values) const { return + // evaluate(values); } + + // /// Calculate log-density for given values `x`. + // double logDensity(const HybridValues &values) const; + /** * @brief Prune the decision tree of Gaussian factors as per the discrete * `decisionTree`. @@ -186,9 +193,9 @@ class GTSAM_EXPORT GaussianMixture * maintaining the decision tree structure. * * @param sum Decision Tree of Gaussian Factor Graphs - * @return Sum + * @return GaussianFactorGraphTree */ - Sum add(const Sum &sum) const; + GaussianFactorGraphTree add(const GaussianFactorGraphTree &sum) const; /// @} }; diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index 746c883b2..657ef334b 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -100,25 +100,25 @@ double GaussianMixtureFactor::constant(const DiscreteValues &assignment) const { // } /* *******************************************************************************/ -GaussianMixtureFactor::Sum GaussianMixtureFactor::add( - const GaussianMixtureFactor::Sum &sum) const { - using Y = GaussianMixtureFactor::GraphAndConstant; +GaussianFactorGraphTree GaussianMixtureFactor::add( + const GaussianFactorGraphTree &sum) const { + using Y = GraphAndConstant; auto add = [](const Y &graph1, const Y &graph2) { auto result = graph1.graph; result.push_back(graph2.graph); return Y(result, graph1.constant + graph2.constant); }; - const Sum tree = asGaussianFactorGraphTree(); + const auto tree = asGaussianFactorGraphTree(); return sum.empty() ? tree : sum.apply(tree, add); } /* *******************************************************************************/ -GaussianMixtureFactor::Sum GaussianMixtureFactor::asGaussianFactorGraphTree() +GaussianFactorGraphTree GaussianMixtureFactor::asGaussianFactorGraphTree() const { auto wrap = [](const FactorAndConstant &factor_z) { GaussianFactorGraph result; result.push_back(factor_z.factor); - return GaussianMixtureFactor::GraphAndConstant(result, factor_z.constant); + return GraphAndConstant(result, factor_z.constant); }; return {factors_, wrap}; } diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index 69149ac05..04b2414a7 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -72,35 +72,6 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { } }; - /// Gaussian factor graph and log of normalizing constant. - struct GraphAndConstant { - GaussianFactorGraph graph; - double constant; - - GraphAndConstant(const GaussianFactorGraph &graph, double constant) - : graph(graph), constant(constant) {} - - // Check pointer equality. - bool operator==(const GraphAndConstant &other) const { - return graph == other.graph && constant == other.constant; - } - - // Implement GTSAM-style print: - void print(const std::string &s = "Graph: ", - const KeyFormatter &formatter = DefaultKeyFormatter) const { - graph.print(s, formatter); - std::cout << "Constant: " << constant << std::endl; - } - - // Implement GTSAM-style equals: - bool equals(const GraphAndConstant &other, double tol = 1e-9) const { - return graph.equals(other.graph, tol) && - fabs(constant - other.constant) < tol; - } - }; - - using Sum = DecisionTree; - /// typedef for Decision Tree of Gaussian factors and log-constant. using Factors = DecisionTree; using Mixture = DecisionTree; @@ -113,9 +84,9 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { * @brief Helper function to return factors and functional to create a * DecisionTree of Gaussian Factor Graphs. * - * @return Sum (DecisionTree) + * @return GaussianFactorGraphTree */ - Sum asGaussianFactorGraphTree() const; + GaussianFactorGraphTree asGaussianFactorGraphTree() const; public: /// @name Constructors @@ -184,7 +155,7 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { * variables. * @return Sum */ - Sum add(const Sum &sum) const; + GaussianFactorGraphTree add(const GaussianFactorGraphTree &sum) const; /** * @brief Compute error of the GaussianMixtureFactor as a tree. @@ -202,7 +173,8 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { double error(const HybridValues &values) const override; /// Add MixtureFactor to a Sum, syntactic sugar. - friend Sum &operator+=(Sum &sum, const GaussianMixtureFactor &factor) { + friend GaussianFactorGraphTree &operator+=( + GaussianFactorGraphTree &sum, const GaussianMixtureFactor &factor) { sum = factor.add(sum); return sum; } @@ -215,7 +187,6 @@ struct traits : public Testable { }; template <> -struct traits - : public Testable {}; +struct traits : public Testable {}; } // namespace gtsam diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index cd6f181ab..fa144ecb3 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -279,18 +279,31 @@ double HybridBayesNet::evaluate(const HybridValues &values) const { const VectorValues &continuousValues = values.continuous(); double logDensity = 0.0, probability = 1.0; + bool debug = false; // Iterate over each conditional. for (auto &&conditional : *this) { + // TODO: should be delegated to derived classes. if (auto gm = conditional->asMixture()) { const auto component = (*gm)(discreteValues); logDensity += component->logDensity(continuousValues); + if (debug) { + GTSAM_PRINT(continuousValues); + std::cout << "component->logDensity(continuousValues) = " + << component->logDensity(continuousValues) << std::endl; + } } else if (auto gc = conditional->asGaussian()) { // If continuous only, evaluate the probability and multiply. logDensity += gc->logDensity(continuousValues); + if (debug) + std::cout << "gc->logDensity(continuousValues) = " + << gc->logDensity(continuousValues) << std::endl; } else if (auto dc = conditional->asDiscrete()) { // Conditional is discrete-only, so return its probability. probability *= dc->operator()(discreteValues); + if (debug) + std::cout << "dc->operator()(discreteValues) = " + << dc->operator()(discreteValues) << std::endl; } } diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index a28fee8ed..326570d6e 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -21,6 +21,8 @@ #include #include #include +#include +#include #include #include @@ -28,6 +30,36 @@ namespace gtsam { class HybridValues; +/// Gaussian factor graph and log of normalizing constant. +struct GraphAndConstant { + GaussianFactorGraph graph; + double constant; + + GraphAndConstant(const GaussianFactorGraph &graph, double constant) + : graph(graph), constant(constant) {} + + // Check pointer equality. + bool operator==(const GraphAndConstant &other) const { + return graph == other.graph && constant == other.constant; + } + + // Implement GTSAM-style print: + void print(const std::string &s = "Graph: ", + const KeyFormatter &formatter = DefaultKeyFormatter) const { + graph.print(s, formatter); + std::cout << "Constant: " << constant << std::endl; + } + + // Implement GTSAM-style equals: + bool equals(const GraphAndConstant &other, double tol = 1e-9) const { + return graph.equals(other.graph, tol) && + fabs(constant - other.constant) < tol; + } +}; + +/// 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/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index a5d27c188..e8b7eb950 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -59,51 +59,44 @@ namespace gtsam { template class EliminateableFactorGraph; /* ************************************************************************ */ -static GaussianMixtureFactor::Sum addGaussian( - const GaussianMixtureFactor::Sum &sum, +static GaussianFactorGraphTree addGaussian( + const GaussianFactorGraphTree &sum, const GaussianFactor::shared_ptr &factor) { // If the decision tree is not initialized, then initialize it. if (sum.empty()) { GaussianFactorGraph result; result.push_back(factor); - return GaussianMixtureFactor::Sum( - GaussianMixtureFactor::GraphAndConstant(result, 0.0)); + return GaussianFactorGraphTree(GraphAndConstant(result, 0.0)); } else { - auto add = [&factor]( - const GaussianMixtureFactor::GraphAndConstant &graph_z) { + auto add = [&factor](const GraphAndConstant &graph_z) { auto result = graph_z.graph; result.push_back(factor); - return GaussianMixtureFactor::GraphAndConstant(result, graph_z.constant); + return GraphAndConstant(result, graph_z.constant); }; return sum.apply(add); } } /* ************************************************************************ */ -// TODO(dellaert): At the time I though "Sum" was a good name, but coming back -// to it after a while I think "SumFrontals" is better.it's a terrible name. -// Also, the implementation is inconsistent. I think we should just have a -// virtual method in HybridFactor that adds to the "Sum" object, like -// addGaussian. Finally, we need to document why deferredFactors need to be -// added last, which I would undo if possible. -// Implementation-wise, it's probably more efficient to first collect the -// discrete keys, and then loop over all assignments to populate a vector. -GaussianMixtureFactor::Sum HybridGaussianFactorGraph::SumFrontals() const { - // sum out frontals, this is the factor on the separator - gttic(sum); +// TODO(dellaert): We need to document why deferredFactors need to be +// added last, which I would undo if possible. Implementation-wise, 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 { + gttic(assembleGraphTree); - GaussianMixtureFactor::Sum sum; + GaussianFactorGraphTree result; std::vector deferredFactors; for (auto &f : factors_) { // TODO(dellaert): just use a virtual method defined in HybridFactor. if (f->isHybrid()) { if (auto gm = boost::dynamic_pointer_cast(f)) { - sum = gm->add(sum); + result = gm->add(result); } if (auto gm = boost::dynamic_pointer_cast(f)) { - sum = gm->asMixture()->add(sum); + result = gm->asMixture()->add(result); } } else if (f->isContinuous()) { @@ -134,12 +127,12 @@ GaussianMixtureFactor::Sum HybridGaussianFactorGraph::SumFrontals() const { } for (auto &f : deferredFactors) { - sum = addGaussian(sum, f); + result = addGaussian(result, f); } - gttoc(sum); + gttoc(assembleGraphTree); - return sum; + return result; } /* ************************************************************************ */ @@ -192,18 +185,14 @@ 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. -GaussianMixtureFactor::Sum removeEmpty(const GaussianMixtureFactor::Sum &sum) { - auto emptyGaussian = [](const GaussianMixtureFactor::GraphAndConstant - &graph_z) { +GaussianFactorGraphTree removeEmpty(const GaussianFactorGraphTree &sum) { + auto emptyGaussian = [](const GraphAndConstant &graph_z) { bool hasNull = std::any_of(graph_z.graph.begin(), graph_z.graph.end(), [](const GaussianFactor::shared_ptr &ptr) { return !ptr; }); - return hasNull - ? GaussianMixtureFactor::GraphAndConstant{GaussianFactorGraph(), - 0.0} - : graph_z; + return hasNull ? GraphAndConstant{GaussianFactorGraph(), 0.0} : graph_z; }; - return GaussianMixtureFactor::Sum(sum, emptyGaussian); + return GaussianFactorGraphTree(sum, emptyGaussian); } /* ************************************************************************ */ static std::pair @@ -218,17 +207,16 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // Collect all the factors to create a set of Gaussian factor graphs in a // decision tree indexed by all discrete keys involved. - GaussianMixtureFactor::Sum sum = factors.SumFrontals(); + GaussianFactorGraphTree sum = factors.assembleGraphTree(); - // TODO(dellaert): does SumFrontals not guarantee we do not need this? + // TODO(dellaert): does assembleGraphTree not guarantee we do not need this? sum = removeEmpty(sum); using EliminationPair = std::pair, GaussianMixtureFactor::FactorAndConstant>; // This is the elimination method on the leaf nodes - auto eliminate = [&](const GaussianMixtureFactor::GraphAndConstant &graph_z) - -> EliminationPair { + auto eliminate = [&](const GraphAndConstant &graph_z) -> EliminationPair { if (graph_z.graph.empty()) { return {nullptr, {nullptr, 0.0}}; } @@ -247,7 +235,15 @@ hybridElimination(const HybridGaussianFactorGraph &factors, #endif // Get the log of the log normalization constant inverse. - double logZ = graph_z.constant - conditional->logNormalizationConstant(); + double logZ = -conditional->logNormalizationConstant(); + + // IF this is the last continuous variable to eliminated, we need to + // calculate the error here: the value of all factors at the mean, see + // ml_map_rao.pdf. + if (continuousSeparator.empty()) { + const auto posterior_mean = conditional->solve(VectorValues()); + logZ += graph_z.graph.error(posterior_mean); + } return {conditional, {newFactor, logZ}}; }; diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index ed3ada2ff..144d144bb 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -245,7 +245,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. */ - GaussianMixtureFactor::Sum SumFrontals() const; + GaussianFactorGraphTree assembleGraphTree() const; /// @} }; diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 9dc1c9576..16b33a0d5 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -80,7 +80,7 @@ TEST(GaussianMixtureFactor, Sum) { // Create sum of two mixture factors: it will be a decision tree now on both // discrete variables m1 and m2: - GaussianMixtureFactor::Sum sum; + GaussianFactorGraphTree sum; sum += mixtureFactorA; sum += mixtureFactorB; diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 7a0653dd5..d048c0e35 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -615,15 +615,16 @@ TEST(HybridGaussianFactorGraph, ErrorAndProbPrimeTree) { } /* ****************************************************************************/ -// Check that SumFrontals assembles Gaussian factor graphs for each assignment. -TEST(HybridGaussianFactorGraph, SumFrontals) { +// Check that assembleGraphTree assembles Gaussian factor graphs for each +// assignment. +TEST(HybridGaussianFactorGraph, assembleGraphTree) { const int num_measurements = 1; const bool deterministic = true; auto fg = tiny::createHybridGaussianFactorGraph(num_measurements, deterministic); EXPECT_LONGS_EQUAL(3, fg.size()); - auto sum = fg.SumFrontals(); + auto sum = fg.assembleGraphTree(); // Get mixture factor: auto mixture = boost::dynamic_pointer_cast(fg.at(0)); @@ -638,7 +639,7 @@ TEST(HybridGaussianFactorGraph, SumFrontals) { // Expected decision tree with two factor graphs: // f(x0;mode=0)P(x0) and f(x0;mode=1)P(x0) - GaussianMixture::Sum expectedSum{ + GaussianFactorGraphTree expectedSum{ M(0), {GaussianFactorGraph(std::vector{mixture->factor(d0), prior}), mixture->constant(d0)},