From 561bdcf9af0f9eb6fe37f1900035c118b7c74d5b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Sep 2024 13:11:05 -0400 Subject: [PATCH 1/6] HybridGaussianConditional inherits from HybridGaussianFactor --- gtsam/hybrid/HybridGaussianConditional.cpp | 14 +++++++++++++- gtsam/hybrid/HybridGaussianConditional.h | 10 +++++----- 2 files changed, 18 insertions(+), 6 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index df59637aa5..e17fd3afee 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -28,13 +28,25 @@ #include namespace gtsam { +HybridGaussianFactor::FactorValuePairs GetFactorValuePairs( + const HybridGaussianConditional::Conditionals &conditionals) { + auto func = [](const GaussianConditional::shared_ptr &conditional) + -> GaussianFactorValuePair { + double value = 0.0; + if (conditional) { // Check if conditional is pruned + value = conditional->logNormalizationConstant(); + } + return {std::dynamic_pointer_cast(conditional), value}; + }; + return HybridGaussianFactor::FactorValuePairs(conditionals, func); +} HybridGaussianConditional::HybridGaussianConditional( const KeyVector &continuousFrontals, const KeyVector &continuousParents, const DiscreteKeys &discreteParents, const HybridGaussianConditional::Conditionals &conditionals) : BaseFactor(CollectKeys(continuousFrontals, continuousParents), - discreteParents), + discreteParents, GetFactorValuePairs(conditionals)), BaseConditional(continuousFrontals.size()), conditionals_(conditionals) { // Calculate logConstant_ as the maximum of the log constants of the diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index eb2bbb9378..82cf6ec8a2 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -51,13 +51,13 @@ class HybridValues; * @ingroup hybrid */ class GTSAM_EXPORT HybridGaussianConditional - : public HybridFactor, - public Conditional { + : public HybridGaussianFactor, + public Conditional { public: using This = HybridGaussianConditional; - using shared_ptr = std::shared_ptr; - using BaseFactor = HybridFactor; - using BaseConditional = Conditional; + using shared_ptr = std::shared_ptr; + using BaseFactor = HybridGaussianFactor; + using BaseConditional = Conditional; /// typedef for Decision Tree of Gaussian Conditionals using Conditionals = DecisionTree; From 9afbc019f4458b9321947bd6be7a71da43ef30d7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Sep 2024 14:53:40 -0400 Subject: [PATCH 2/6] rename logConstant_ to logNormalizer_ and add test for error with mode-dependent constants --- gtsam/hybrid/HybridGaussianConditional.cpp | 23 +++++++------ gtsam/hybrid/HybridGaussianConditional.h | 5 +-- .../tests/testHybridGaussianConditional.cpp | 33 ++++++++++++++++++- 3 files changed, 48 insertions(+), 13 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index e17fd3afee..e0ae16e82b 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -33,8 +33,10 @@ HybridGaussianFactor::FactorValuePairs GetFactorValuePairs( auto func = [](const GaussianConditional::shared_ptr &conditional) -> GaussianFactorValuePair { double value = 0.0; - if (conditional) { // Check if conditional is pruned - value = conditional->logNormalizationConstant(); + // Check if conditional is pruned + if (conditional) { + // Assign log(|2πΣ|) = -2*log(1 / sqrt(|2πΣ|)) + value = -2.0 * conditional->logNormalizationConstant(); } return {std::dynamic_pointer_cast(conditional), value}; }; @@ -49,14 +51,14 @@ HybridGaussianConditional::HybridGaussianConditional( discreteParents, GetFactorValuePairs(conditionals)), BaseConditional(continuousFrontals.size()), conditionals_(conditionals) { - // Calculate logConstant_ as the maximum of the log constants of the + // Calculate logNormalizer_ as the minimum of the log normalizers of the // conditionals, by visiting the decision tree: - logConstant_ = -std::numeric_limits::infinity(); + logNormalizer_ = std::numeric_limits::infinity(); conditionals_.visit( [this](const GaussianConditional::shared_ptr &conditional) { if (conditional) { - this->logConstant_ = std::max( - this->logConstant_, conditional->logNormalizationConstant()); + this->logNormalizer_ = std::min( + this->logNormalizer_, -conditional->logNormalizationConstant()); } }); } @@ -98,7 +100,7 @@ GaussianFactorGraphTree HybridGaussianConditional::asGaussianFactorGraphTree() // First check if conditional has not been pruned if (gc) { const double Cgm_Kgcm = - this->logConstant_ - gc->logNormalizationConstant(); + -gc->logNormalizationConstant() - this->logNormalizer_; // 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) { @@ -169,7 +171,8 @@ void HybridGaussianConditional::print(const std::string &s, std::cout << "(" << formatter(dk.first) << ", " << dk.second << "), "; } std::cout << std::endl - << " logNormalizationConstant: " << logConstant_ << std::endl + << " logNormalizationConstant: " << logNormalizationConstant() + << std::endl << std::endl; conditionals_.print( "", [&](Key k) { return formatter(k); }, @@ -228,7 +231,7 @@ std::shared_ptr HybridGaussianConditional::likelihood( -> GaussianFactorValuePair { const auto likelihood_m = conditional->likelihood(given); const double Cgm_Kgcm = - logConstant_ - conditional->logNormalizationConstant(); + -conditional->logNormalizationConstant() - logNormalizer_; if (Cgm_Kgcm == 0.0) { return {likelihood_m, 0.0}; } else { @@ -342,7 +345,7 @@ double HybridGaussianConditional::conditionalError( // Check if valid pointer if (conditional) { return conditional->error(continuousValues) + // - logConstant_ - conditional->logNormalizationConstant(); + -conditional->logNormalizationConstant() - logNormalizer_; } else { // If not valid, pointer, it means this conditional was pruned, // so we return maximum error. diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index 82cf6ec8a2..434750bc90 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -64,7 +64,8 @@ class GTSAM_EXPORT HybridGaussianConditional private: Conditionals conditionals_; ///< a decision tree of Gaussian conditionals. - double logConstant_; ///< log of the normalization constant. + double logNormalizer_; ///< log of the normalization constant + ///< (log(\sqrt(|2πΣ|))). /** * @brief Convert a HybridGaussianConditional of conditionals into @@ -149,7 +150,7 @@ class GTSAM_EXPORT HybridGaussianConditional /// The log normalization constant is max of the the individual /// log-normalization constants. - double logNormalizationConstant() const override { return logConstant_; } + double logNormalizationConstant() const override { return -logNormalizer_; } /** * Create a likelihood factor for a hybrid Gaussian conditional, diff --git a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp index 406203db84..70cc55712e 100644 --- a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp @@ -100,7 +100,7 @@ TEST(HybridGaussianConditional, Error) { auto actual = hybrid_conditional.errorTree(vv); // Check result. - std::vector discrete_keys = {mode}; + DiscreteKeys discrete_keys{mode}; std::vector leaves = {conditionals[0]->error(vv), conditionals[1]->error(vv)}; AlgebraicDecisionTree expected(discrete_keys, leaves); @@ -172,6 +172,37 @@ TEST(HybridGaussianConditional, ContinuousParents) { EXPECT(continuousParentKeys[0] == X(0)); } +/* ************************************************************************* */ +/// Check error with mode dependent constants. +TEST(HybridGaussianConditional, Error2) { + using namespace mode_dependent_constants; + auto actual = mixture.errorTree(vv); + + // Check result. + DiscreteKeys discrete_keys{mode}; + double logNormalizer0 = -conditionals[0]->logNormalizationConstant(); + double logNormalizer1 = -conditionals[1]->logNormalizationConstant(); + double minLogNormalizer = std::min(logNormalizer0, logNormalizer1); + + // Expected error is e(X) + log(|2πΣ|). + // We normalize log(|2πΣ|) with min(logNormalizers) so it is non-negative. + std::vector leaves = { + conditionals[0]->error(vv) + logNormalizer0 - minLogNormalizer, + conditionals[1]->error(vv) + logNormalizer1 - minLogNormalizer}; + AlgebraicDecisionTree expected(discrete_keys, leaves); + + EXPECT(assert_equal(expected, actual, 1e-6)); + + // Check for non-tree version. + for (size_t mode : {0, 1}) { + const HybridValues hv{vv, {{M(0), mode}}}; + EXPECT_DOUBLES_EQUAL(conditionals[mode]->error(vv) - + conditionals[mode]->logNormalizationConstant() - + minLogNormalizer, + mixture.error(hv), 1e-8); + } +} + /* ************************************************************************* */ /// Check that the likelihood is proportional to the conditional density given /// the measurements. From 5c72b5d0bc3d114e3887debd559749a6b0a674b9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Sep 2024 18:38:46 -0400 Subject: [PATCH 3/6] update naming --- gtsam/hybrid/tests/testHybridGaussianConditional.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp index 70cc55712e..dde493685b 100644 --- a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp @@ -176,7 +176,7 @@ TEST(HybridGaussianConditional, ContinuousParents) { /// Check error with mode dependent constants. TEST(HybridGaussianConditional, Error2) { using namespace mode_dependent_constants; - auto actual = mixture.errorTree(vv); + auto actual = hybrid_conditional.errorTree(vv); // Check result. DiscreteKeys discrete_keys{mode}; @@ -199,7 +199,7 @@ TEST(HybridGaussianConditional, Error2) { EXPECT_DOUBLES_EQUAL(conditionals[mode]->error(vv) - conditionals[mode]->logNormalizationConstant() - minLogNormalizer, - mixture.error(hv), 1e-8); + hybrid_conditional.error(hv), 1e-8); } } From 97a7121c37e1aa193ff12af9da9f117b3f806d54 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 20 Sep 2024 02:34:40 -0400 Subject: [PATCH 4/6] remove duplicate method --- gtsam/hybrid/HybridGaussianConditional.cpp | 15 --------------- gtsam/hybrid/HybridGaussianConditional.h | 13 +++---------- 2 files changed, 3 insertions(+), 25 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index e0ae16e82b..c31a9836f6 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -78,21 +78,6 @@ HybridGaussianConditional::HybridGaussianConditional( DiscreteKeys{discreteParent}, Conditionals({discreteParent}, conditionals)) {} -/* *******************************************************************************/ -// TODO(dellaert): This is copy/paste: HybridGaussianConditional should be -// derived from HybridGaussianFactor, no? -GaussianFactorGraphTree HybridGaussianConditional::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 HybridGaussianConditional::asGaussianFactorGraphTree() const { diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index 434750bc90..5c3e500bd1 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -108,8 +108,9 @@ class GTSAM_EXPORT HybridGaussianConditional const Conditionals &conditionals); /** - * @brief Make a Hybrid Gaussian Conditional from a vector of Gaussian conditionals. - * The DecisionTree-based constructor is preferred over this one. + * @brief Make a Hybrid Gaussian Conditional from a vector of Gaussian + * conditionals. The DecisionTree-based constructor is preferred over this + * one. * * @param continuousFrontals The continuous frontal variables * @param continuousParents The continuous parent variables @@ -233,14 +234,6 @@ class GTSAM_EXPORT HybridGaussianConditional */ void prune(const DecisionTreeFactor &discreteProbs); - /** - * @brief Merge the Gaussian Factor Graphs in `this` and `sum` while - * maintaining the decision tree structure. - * - * @param sum Decision Tree of Gaussian Factor Graphs - * @return GaussianFactorGraphTree - */ - GaussianFactorGraphTree add(const GaussianFactorGraphTree &sum) const; /// @} private: From f3bfe7e1a1e8066fcf0cb6a69eeedbe0334f955d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 20 Sep 2024 10:00:49 -0400 Subject: [PATCH 5/6] consistent naming --- gtsam/hybrid/HybridGaussianConditional.cpp | 14 +++++++------- gtsam/hybrid/HybridGaussianConditional.h | 7 ++++--- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index c31a9836f6..b408eea9c0 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -51,14 +51,14 @@ HybridGaussianConditional::HybridGaussianConditional( discreteParents, GetFactorValuePairs(conditionals)), BaseConditional(continuousFrontals.size()), conditionals_(conditionals) { - // Calculate logNormalizer_ as the minimum of the log normalizers of the + // Calculate logConstant_ as the minimum of the log normalizers of the // conditionals, by visiting the decision tree: - logNormalizer_ = std::numeric_limits::infinity(); + logConstant_ = std::numeric_limits::infinity(); conditionals_.visit( [this](const GaussianConditional::shared_ptr &conditional) { if (conditional) { - this->logNormalizer_ = std::min( - this->logNormalizer_, -conditional->logNormalizationConstant()); + this->logConstant_ = std::min( + this->logConstant_, -conditional->logNormalizationConstant()); } }); } @@ -85,7 +85,7 @@ GaussianFactorGraphTree HybridGaussianConditional::asGaussianFactorGraphTree() // First check if conditional has not been pruned if (gc) { const double Cgm_Kgcm = - -gc->logNormalizationConstant() - this->logNormalizer_; + -this->logConstant_ - gc->logNormalizationConstant(); // 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) { @@ -216,7 +216,7 @@ std::shared_ptr HybridGaussianConditional::likelihood( -> GaussianFactorValuePair { const auto likelihood_m = conditional->likelihood(given); const double Cgm_Kgcm = - -conditional->logNormalizationConstant() - logNormalizer_; + -logConstant_ - conditional->logNormalizationConstant(); if (Cgm_Kgcm == 0.0) { return {likelihood_m, 0.0}; } else { @@ -330,7 +330,7 @@ double HybridGaussianConditional::conditionalError( // Check if valid pointer if (conditional) { return conditional->error(continuousValues) + // - -conditional->logNormalizationConstant() - logNormalizer_; + -logConstant_ - conditional->logNormalizationConstant(); } else { // If not valid, pointer, it means this conditional was pruned, // so we return maximum error. diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index 5c3e500bd1..72a9994729 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -64,8 +64,9 @@ class GTSAM_EXPORT HybridGaussianConditional private: Conditionals conditionals_; ///< a decision tree of Gaussian conditionals. - double logNormalizer_; ///< log of the normalization constant - ///< (log(\sqrt(|2πΣ|))). + ///< Negative-log of the normalization constant (log(\sqrt(|2πΣ|))). + ///< Take advantage of the neg-log space so everything is a minimization + double logConstant_; /** * @brief Convert a HybridGaussianConditional of conditionals into @@ -151,7 +152,7 @@ class GTSAM_EXPORT HybridGaussianConditional /// The log normalization constant is max of the the individual /// log-normalization constants. - double logNormalizationConstant() const override { return -logNormalizer_; } + double logNormalizationConstant() const override { return -logConstant_; } /** * Create a likelihood factor for a hybrid Gaussian conditional, From 245f3e042e6e62154657d0d65edbae5292fd0d83 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 20 Sep 2024 12:20:49 -0400 Subject: [PATCH 6/6] fix conversion --- gtsam/hybrid/HybridGaussianConditional.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index b408eea9c0..0b1dc53377 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -35,8 +35,8 @@ HybridGaussianFactor::FactorValuePairs GetFactorValuePairs( double value = 0.0; // Check if conditional is pruned if (conditional) { - // Assign log(|2πΣ|) = -2*log(1 / sqrt(|2πΣ|)) - value = -2.0 * conditional->logNormalizationConstant(); + // Assign log(\sqrt(|2πΣ|)) = -log(1 / sqrt(|2πΣ|)) + value = -conditional->logNormalizationConstant(); } return {std::dynamic_pointer_cast(conditional), value}; };