From 7695fd6de3fc00cf7d348dc2f30cd6e6f4548f1a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 20 Nov 2023 13:24:39 -0500 Subject: [PATCH 01/71] Improved HybridBayesNet::optimize with proper model selection --- gtsam/hybrid/HybridBayesNet.cpp | 60 +++++++++++++++++++++++++++++++-- 1 file changed, 57 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 31177ddb7..ba869a6f5 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -220,15 +220,69 @@ GaussianBayesNet HybridBayesNet::choose( /* ************************************************************************* */ HybridValues HybridBayesNet::optimize() const { // Collect all the discrete factors to compute MPE - DiscreteBayesNet discrete_bn; + DiscreteFactorGraph discrete_fg; + VectorValues continuousValues; + for (auto &&conditional : *this) { if (conditional->isDiscrete()) { - discrete_bn.push_back(conditional->asDiscrete()); + discrete_fg.push_back(conditional->asDiscrete()); + } else { + /* + Perform the integration of L(X;M, Z)P(X|M) which is the model selection + term. + TODO(Varun) Write better comments detailing the whole process. + */ + if (conditional->isContinuous()) { + auto gc = conditional->asGaussian(); + for (GaussianConditional::const_iterator frontal = gc->beginFrontals(); + frontal != gc->endFrontals(); ++frontal) { + continuousValues.insert_or_assign(*frontal, + Vector::Zero(gc->getDim(frontal))); + } + } else if (conditional->isHybrid()) { + auto gm = conditional->asMixture(); + gm->conditionals().apply( + [&continuousValues](const GaussianConditional::shared_ptr &gc) { + if (gc) { + for (GaussianConditional::const_iterator frontal = gc->begin(); + frontal != gc->end(); ++frontal) { + continuousValues.insert_or_assign( + *frontal, Vector::Zero(gc->getDim(frontal))); + } + } + return gc; + }); + + DecisionTree error = gm->error(continuousValues); + + // Functional to take error and compute the probability + auto integrate = [&gm](const double &error) { + // q(mu; M, Z) = exp(-error) + // k = 1.0 / sqrt((2*pi)^n*det(Sigma)) + // thus, q*sqrt(|2*pi*Sigma|) = q/k = exp(log(q) - log(k)) + // = exp(-error - log(k)) + double prob = std::exp(-error - gm->logNormalizationConstant()); + if (prob > 1e-12) { + return prob; + } else { + return 1.0; + } + }; + AlgebraicDecisionTree model_selection = + DecisionTree(error, integrate); + + std::cout << "\n\nmodel selection"; + model_selection.print("", DefaultKeyFormatter); + discrete_fg.push_back( + DecisionTreeFactor(gm->discreteKeys(), model_selection)); + } } } // Solve for the MPE - DiscreteValues mpe = DiscreteFactorGraph(discrete_bn).optimize(); + discrete_fg.print(); + DiscreteValues mpe = discrete_fg.optimize(); + mpe.print("mpe"); // Given the MPE, compute the optimal continuous values. return HybridValues(optimize(mpe), mpe); From 39f7ac20a1d8a0bdaff47d31dd61331188a5a14c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 20 Nov 2023 13:28:05 -0500 Subject: [PATCH 02/71] handle nullptrsin GaussianMixture::error --- gtsam/hybrid/GaussianMixture.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 753e35bf0..a1ba167f5 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -316,8 +316,15 @@ AlgebraicDecisionTree GaussianMixture::logProbability( AlgebraicDecisionTree GaussianMixture::error( const VectorValues &continuousValues) const { auto errorFunc = [&](const GaussianConditional::shared_ptr &conditional) { - return conditional->error(continuousValues) + // - logConstant_ - conditional->logNormalizationConstant(); + // Check if valid pointer + if (conditional) { + return conditional->error(continuousValues) + // + logConstant_ - conditional->logNormalizationConstant(); + } else { + // If not valid, pointer, it means this conditional was pruned, + // so we return maximum error. + return std::numeric_limits::max(); + } }; DecisionTree errorTree(conditionals_, errorFunc); return errorTree; From c374a26b45ed1f25c597ad08d12594ee4414282e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 20 Nov 2023 13:31:07 -0500 Subject: [PATCH 03/71] nicer HybridBayesNet::optimize with normalized errors --- gtsam/hybrid/HybridBayesNet.cpp | 75 ++++++++++++++++++++++----------- 1 file changed, 51 insertions(+), 24 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index ba869a6f5..cd1157576 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -223,22 +223,38 @@ HybridValues HybridBayesNet::optimize() const { DiscreteFactorGraph discrete_fg; VectorValues continuousValues; + // Error values for each hybrid factor + AlgebraicDecisionTree error(0.0); + std::set discreteKeySet; + for (auto &&conditional : *this) { if (conditional->isDiscrete()) { discrete_fg.push_back(conditional->asDiscrete()); } else { /* - Perform the integration of L(X;M, Z)P(X|M) which is the model selection - term. - TODO(Varun) Write better comments detailing the whole process. + Perform the integration of L(X;M,Z)P(X|M) + which is the model selection term. + + By Bayes' rule, P(X|M) ∝ L(X;M,Z)P(X|M), + hence L(X;M,Z)P(X|M) is the unnormalized probabilty of + the joint Gaussian distribution. + + This can be computed by multiplying all the exponentiaed errors + of each of the conditionals, which we do below in hybrid case. */ if (conditional->isContinuous()) { + /* + If we are here, it means there are no discrete variables in + the Bayes net (due to strong elimination ordering). + This is a continuous-only problem hence model selection doesn't matter. + */ auto gc = conditional->asGaussian(); for (GaussianConditional::const_iterator frontal = gc->beginFrontals(); frontal != gc->endFrontals(); ++frontal) { continuousValues.insert_or_assign(*frontal, Vector::Zero(gc->getDim(frontal))); } + } else if (conditional->isHybrid()) { auto gm = conditional->asMixture(); gm->conditionals().apply( @@ -253,36 +269,47 @@ HybridValues HybridBayesNet::optimize() const { return gc; }); - DecisionTree error = gm->error(continuousValues); + /* + To perform model selection, we need: + q(mu; M, Z) * sqrt((2*pi)^n*det(Sigma)) - // Functional to take error and compute the probability - auto integrate = [&gm](const double &error) { - // q(mu; M, Z) = exp(-error) - // k = 1.0 / sqrt((2*pi)^n*det(Sigma)) - // thus, q*sqrt(|2*pi*Sigma|) = q/k = exp(log(q) - log(k)) - // = exp(-error - log(k)) - double prob = std::exp(-error - gm->logNormalizationConstant()); - if (prob > 1e-12) { - return prob; - } else { - return 1.0; - } + If q(mu; M, Z) = exp(-error) & k = 1.0 / sqrt((2*pi)^n*det(Sigma)) + thus, q*sqrt(|2*pi*Sigma|) = q/k = exp(log(q/k)) + = exp(log(q) - log(k)) = exp(-error - log(k)) + = exp(-(error + log(k))) + + So let's compute (error + log(k)) and exponentiate later + */ + error = error + gm->error(continuousValues); + + // Add the logNormalization constant to the error + // Also compute the mean for normalization (for numerical stability) + double mean = 0.0; + auto addConstant = [&gm, &mean](const double &error) { + double e = error + gm->logNormalizationConstant(); + mean += e; + return e; }; - AlgebraicDecisionTree model_selection = - DecisionTree(error, integrate); + error = error.apply(addConstant); + // Normalize by the mean + error = error.apply([&mean](double x) { return x / mean; }); - std::cout << "\n\nmodel selection"; - model_selection.print("", DefaultKeyFormatter); - discrete_fg.push_back( - DecisionTreeFactor(gm->discreteKeys(), model_selection)); + // Include the discrete keys + std::copy(gm->discreteKeys().begin(), gm->discreteKeys().end(), + std::inserter(discreteKeySet, discreteKeySet.end())); } } } + AlgebraicDecisionTree model_selection = DecisionTree( + error, [](const double &error) { return std::exp(-error); }); + + discrete_fg.push_back(DecisionTreeFactor( + DiscreteKeys(discreteKeySet.begin(), discreteKeySet.end()), + model_selection)); + // Solve for the MPE - discrete_fg.print(); DiscreteValues mpe = discrete_fg.optimize(); - mpe.print("mpe"); // Given the MPE, compute the optimal continuous values. return HybridValues(optimize(mpe), mpe); From 50670da07ceeb2947ca75d15cd3b52a6fd0adbb9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 12 Dec 2023 05:57:23 -0500 Subject: [PATCH 04/71] HybridValues formatting --- gtsam/hybrid/HybridValues.h | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/gtsam/hybrid/HybridValues.h b/gtsam/hybrid/HybridValues.h index 40d4b5e92..09e7d773c 100644 --- a/gtsam/hybrid/HybridValues.h +++ b/gtsam/hybrid/HybridValues.h @@ -12,6 +12,7 @@ /** * @file HybridValues.h * @date Jul 28, 2022 + * @author Varun Agrawal * @author Shangjie Xue */ @@ -54,13 +55,13 @@ class GTSAM_EXPORT HybridValues { HybridValues() = default; /// Construct from DiscreteValues and VectorValues. - HybridValues(const VectorValues &cv, const DiscreteValues &dv) - : continuous_(cv), discrete_(dv){} + HybridValues(const VectorValues& cv, const DiscreteValues& dv) + : continuous_(cv), discrete_(dv) {} /// Construct from all values types. HybridValues(const VectorValues& cv, const DiscreteValues& dv, const Values& v) - : continuous_(cv), discrete_(dv), nonlinear_(v){} + : continuous_(cv), discrete_(dv), nonlinear_(v) {} /// @} /// @name Testable @@ -101,9 +102,7 @@ class GTSAM_EXPORT HybridValues { bool existsDiscrete(Key j) { return (discrete_.find(j) != discrete_.end()); } /// Check whether a variable with key \c j exists in values. - bool existsNonlinear(Key j) { - return nonlinear_.exists(j); - } + bool existsNonlinear(Key j) { return nonlinear_.exists(j); } /// Check whether a variable with key \c j exists. bool exists(Key j) { @@ -128,9 +127,7 @@ class GTSAM_EXPORT HybridValues { } /// insert_or_assign() , similar to Values.h - void insert_or_assign(Key j, size_t value) { - discrete_[j] = value; - } + void insert_or_assign(Key j, size_t value) { discrete_[j] = value; } /** Insert all continuous values from \c values. Throws an invalid_argument * exception if any keys to be inserted are already used. */ From af490e9ffcbab044637f9a9b1885cfc4e751332c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 12 Dec 2023 06:26:57 -0500 Subject: [PATCH 05/71] sum and normalize helper methods for the AlgebraicDecisionTree --- gtsam/discrete/AlgebraicDecisionTree.h | 19 +++++++++++++++++++ gtsam/hybrid/HybridBayesNet.cpp | 13 +++++++------ 2 files changed, 26 insertions(+), 6 deletions(-) diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index 9f55f3b63..1e5dc848b 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -196,6 +196,25 @@ namespace gtsam { return this->apply(g, &Ring::div); } + /// Compute sum of all values + double sum() const { + double sum = 0; + auto visitor = [&](int y) { sum += y; }; + this->visit(visitor); + return sum; + } + + /** + * @brief Helper method to perform normalization such that all leaves in the + * tree sum to 1 + * + * @param sum + * @return AlgebraicDecisionTree + */ + AlgebraicDecisionTree normalize(double sum) const { + return this->apply([&sum](const double& x) { return x / sum; }); + } + /** sum out variable */ AlgebraicDecisionTree sum(const L& label, size_t cardinality) const { return this->combine(label, cardinality, &Ring::add); diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index cd1157576..eea51d329 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -283,16 +283,17 @@ HybridValues HybridBayesNet::optimize() const { error = error + gm->error(continuousValues); // Add the logNormalization constant to the error - // Also compute the mean for normalization (for numerical stability) - double mean = 0.0; - auto addConstant = [&gm, &mean](const double &error) { + // Also compute the sum for discrete probability normalization + // (normalization trick for numerical stability) + double sum = 0.0; + auto addConstant = [&gm, &sum](const double &error) { double e = error + gm->logNormalizationConstant(); - mean += e; + sum += e; return e; }; error = error.apply(addConstant); - // Normalize by the mean - error = error.apply([&mean](double x) { return x / mean; }); + // Normalize by the sum + error = error.normalize(sum); // Include the discrete keys std::copy(gm->discreteKeys().begin(), gm->discreteKeys().end(), From c004bd8df0775bdb14f6df46334545cf1ac03b90 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 12 Dec 2023 19:15:24 -0500 Subject: [PATCH 06/71] test for differing covariances --- gtsam/hybrid/tests/testMixtureFactor.cpp | 70 ++++++++++++++++++++++++ 1 file changed, 70 insertions(+) diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp index 67a7fd8ae..50bf2d4b4 100644 --- a/gtsam/hybrid/tests/testMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testMixtureFactor.cpp @@ -18,6 +18,9 @@ #include #include +#include +#include +#include #include #include #include @@ -114,6 +117,73 @@ TEST(MixtureFactor, Dim) { EXPECT_LONGS_EQUAL(1, mixtureFactor.dim()); } +/* ************************************************************************* */ +// Test components with differing covariances +TEST(MixtureFactor, DifferentCovariances) { + DiscreteKey m1(M(1), 2); + + Values values; + double x1 = 1.0, x2 = 1.0; + values.insert(X(1), x1); + values.insert(X(2), x2); + + double between = 0.0; + + auto model0 = noiseModel::Isotropic::Sigma(1, 1e2); + auto model1 = noiseModel::Isotropic::Sigma(1, 1e-2); + auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); + + auto f0 = + std::make_shared>(X(1), X(2), between, model0); + auto f1 = + std::make_shared>(X(1), X(2), between, model1); + std::vector factors{f0, f1}; + + // Create via toFactorGraph + using symbol_shorthand::Z; + Matrix H0_1, H0_2, H1_1, H1_2; + Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2); + std::vector> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/}, + // + {X(1), H0_1 /*Sp1*/}, + {X(2), H0_2 /*Tp2*/}}; + + Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2); + std::vector> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/}, + // + {X(1), H1_1 /*Sp1*/}, + {X(2), H1_2 /*Tp2*/}}; + auto gm = new gtsam::GaussianMixture( + {Z(1)}, {X(1), X(2)}, {m1}, + {std::make_shared(terms0, 1, -d0, model0), + std::make_shared(terms1, 1, -d1, model1)}); + gtsam::HybridBayesNet bn2; + bn2.emplace_back(gm); + + gtsam::VectorValues measurements; + measurements.insert(Z(1), gtsam::Z_1x1); + // Create FG with single GaussianMixtureFactor + auto mixture_fg = bn2.toFactorGraph(measurements); + + // Linearized prior factor on X1 + auto prior = PriorFactor(X(1), x1, prior_noise).linearize(values); + mixture_fg.push_back(prior); + + auto hbn = mixture_fg.eliminateSequential(); + // hbn->print("\n\nfinal bayes net"); + + HybridValues actual_values = hbn->optimize(); + + VectorValues cv; + cv.insert(X(1), Vector1(0.0)); + cv.insert(X(2), Vector1(0.0)); + DiscreteValues dv; + dv.insert({M(1), 1}); + HybridValues expected_values(cv, dv); + + EXPECT(assert_equal(expected_values, actual_values)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 7b56c96b430cf9d828016749c3299366a8a67b0e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 15 Dec 2023 15:19:48 -0500 Subject: [PATCH 07/71] differing means test --- gtsam/hybrid/tests/testMixtureFactor.cpp | 45 ++++++++++++++++++++++++ 1 file changed, 45 insertions(+) diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp index 50bf2d4b4..4bb716540 100644 --- a/gtsam/hybrid/tests/testMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testMixtureFactor.cpp @@ -117,6 +117,51 @@ TEST(MixtureFactor, Dim) { EXPECT_LONGS_EQUAL(1, mixtureFactor.dim()); } +/* ************************************************************************* */ +// Test components with differing means +TEST(MixtureFactor, DifferentMeans) { + DiscreteKey m1(M(1), 2), m2(M(2), 2); + + Values values; + double x1 = 0.0, x2 = 1.75, x3 = 2.60; + values.insert(X(1), x1); + values.insert(X(2), x2); + values.insert(X(3), x3); + + auto model0 = noiseModel::Isotropic::Sigma(1, 1e-0); + auto model1 = noiseModel::Isotropic::Sigma(1, 1e-0); + auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-0); + + auto f0 = std::make_shared>(X(1), X(2), 0.0, model0); + auto f1 = std::make_shared>(X(1), X(2), 2.0, model1); + std::vector factors{f0, f1}; + + MixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors); + HybridNonlinearFactorGraph hnfg; + hnfg.push_back(mixtureFactor); + + f0 = std::make_shared>(X(2), X(3), 0.0, model0); + f1 = std::make_shared>(X(2), X(3), 2.0, model1); + std::vector factors23{f0, f1}; + hnfg.push_back(MixtureFactor({X(2), X(3)}, {m2}, factors23)); + + auto prior = PriorFactor(X(1), x1, prior_noise); + hnfg.push_back(prior); + + hnfg.emplace_shared>(X(2), 2.0, prior_noise); + + auto hgfg = hnfg.linearize(values); + auto bn = hgfg->eliminateSequential(); + HybridValues actual = bn->optimize(); + + HybridValues expected( + VectorValues{ + {X(1), Vector1(0.0)}, {X(2), Vector1(0.25)}, {X(3), Vector1(-0.6)}}, + DiscreteValues{{M(1), 1}, {M(2), 0}}); + + EXPECT(assert_equal(expected, actual)); +} + /* ************************************************************************* */ // Test components with differing covariances TEST(MixtureFactor, DifferentCovariances) { From e549a9b41fb6980e4fb1fbcf89df40b927797d9c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 15 Dec 2023 15:21:25 -0500 Subject: [PATCH 08/71] normalize model selection term --- gtsam/hybrid/HybridBayesNet.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index eea51d329..f2cac485d 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -239,7 +239,7 @@ HybridValues HybridBayesNet::optimize() const { hence L(X;M,Z)P(X|M) is the unnormalized probabilty of the joint Gaussian distribution. - This can be computed by multiplying all the exponentiaed errors + This can be computed by multiplying all the exponentiated errors of each of the conditionals, which we do below in hybrid case. */ if (conditional->isContinuous()) { @@ -288,7 +288,7 @@ HybridValues HybridBayesNet::optimize() const { double sum = 0.0; auto addConstant = [&gm, &sum](const double &error) { double e = error + gm->logNormalizationConstant(); - sum += e; + sum += std::abs(e); return e; }; error = error.apply(addConstant); @@ -302,12 +302,17 @@ HybridValues HybridBayesNet::optimize() const { } } + double min_log = error.min(); AlgebraicDecisionTree model_selection = DecisionTree( - error, [](const double &error) { return std::exp(-error); }); + error, [&min_log](const double &x) { return std::exp(-(x - min_log)); }); + model_selection = model_selection + exp(-min_log); - discrete_fg.push_back(DecisionTreeFactor( - DiscreteKeys(discreteKeySet.begin(), discreteKeySet.end()), - model_selection)); + // Only add model_selection if we have discrete keys + if (discreteKeySet.size() > 0) { + discrete_fg.push_back(DecisionTreeFactor( + DiscreteKeys(discreteKeySet.begin(), discreteKeySet.end()), + model_selection)); + } // Solve for the MPE DiscreteValues mpe = discrete_fg.optimize(); From b2638c8698689545c0b0825a0a7fd3d1bd7fc0ba Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 17 Dec 2023 15:14:03 -0500 Subject: [PATCH 09/71] max and min functions for AlgebraicDecisionTree --- gtsam/discrete/AlgebraicDecisionTree.h | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index 1e5dc848b..1a6358680 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -199,7 +199,7 @@ namespace gtsam { /// Compute sum of all values double sum() const { double sum = 0; - auto visitor = [&](int y) { sum += y; }; + auto visitor = [&](double y) { sum += y; }; this->visit(visitor); return sum; } @@ -215,6 +215,22 @@ namespace gtsam { return this->apply([&sum](const double& x) { return x / sum; }); } + /// Find the minimum values amongst all leaves + double min() const { + double min = std::numeric_limits::max(); + auto visitor = [&](double x) { min = x < min ? x : min; }; + this->visit(visitor); + return min; + } + + /// Find the maximum values amongst all leaves + double max() const { + double max = std::numeric_limits::min(); + auto visitor = [&](double x) { max = x > max ? x : max; }; + this->visit(visitor); + return max; + } + /** sum out variable */ AlgebraicDecisionTree sum(const L& label, size_t cardinality) const { return this->combine(label, cardinality, &Ring::add); From 6f09be51cbf145590b3ba545f59fdbc5aa9c1718 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 17 Dec 2023 18:55:15 -0500 Subject: [PATCH 10/71] error normalization and log-sum-exp trick --- gtsam/hybrid/HybridBayesNet.cpp | 7 ++++--- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 23 ++++++++++++++++------ 2 files changed, 21 insertions(+), 9 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index f2cac485d..e48b3faf7 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -303,9 +303,10 @@ HybridValues HybridBayesNet::optimize() const { } double min_log = error.min(); - AlgebraicDecisionTree model_selection = DecisionTree( - error, [&min_log](const double &x) { return std::exp(-(x - min_log)); }); - model_selection = model_selection + exp(-min_log); + AlgebraicDecisionTree model_selection = + DecisionTree(error, [&min_log](const double &x) { + return std::exp(-(x - min_log)) * exp(-min_log); + }); // Only add model_selection if we have discrete keys if (discreteKeySet.size() > 0) { diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 467cff710..dfff3d4f3 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -323,18 +323,29 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // If there are no more continuous parents, then we create a // DiscreteFactor here, with the error for each discrete choice. - // Integrate the probability mass in the last continuous conditional using - // the unnormalized probability q(μ;m) = exp(-error(μ;m)) at the mean. - // discrete_probability = exp(-error(μ;m)) * sqrt(det(2π Σ_m)) - auto probability = [&](const Result &pair) -> double { + // Compute the probability q(μ;m) = exp(-error(μ;m)) * sqrt(det(2π Σ_m)) + // from the residual error at the mean μ. + // The residual error contains no keys, and only depends on the discrete + // separator if present. + auto logProbability = [&](const Result &pair) -> double { + // auto probability = [&](const Result &pair) -> double { static const VectorValues kEmpty; // If the factor is not null, it has no keys, just contains the residual. const auto &factor = pair.second; if (!factor) return 1.0; // TODO(dellaert): not loving this. - return exp(-factor->error(kEmpty)) / pair.first->normalizationConstant(); + + // exp(-factor->error(kEmpty)) / pair.first->normalizationConstant(); + return -factor->error(kEmpty) - pair.first->logNormalizationConstant(); }; - DecisionTree probabilities(eliminationResults, probability); + AlgebraicDecisionTree logProbabilities( + DecisionTree(eliminationResults, logProbability)); + + // Perform normalization + double max_log = logProbabilities.max(); + DecisionTree probabilities( + logProbabilities, + [&max_log](const double x) { return exp(x - max_log) * exp(max_log); }); return { std::make_shared(gaussianMixture), From 36604297d7db6f8a0c299f782a157c72f1a70cf9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 18 Dec 2023 14:25:19 -0500 Subject: [PATCH 11/71] handle numerical instability --- gtsam/discrete/AlgebraicDecisionTree.h | 3 ++- gtsam/hybrid/HybridBayesNet.cpp | 28 ++++++++++----------- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 7 +++--- gtsam/hybrid/tests/testHybridEstimation.cpp | 1 - gtsam/linear/GaussianConditional.cpp | 6 +++-- 5 files changed, 24 insertions(+), 21 deletions(-) diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index 1a6358680..17385a975 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -225,7 +225,8 @@ namespace gtsam { /// Find the maximum values amongst all leaves double max() const { - double max = std::numeric_limits::min(); + // Get the most negative value + double max = -std::numeric_limits::max(); auto visitor = [&](double x) { max = x > max ? x : max; }; this->visit(visitor); return max; diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index e48b3faf7..2b0b11e36 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -274,26 +274,26 @@ HybridValues HybridBayesNet::optimize() const { q(mu; M, Z) * sqrt((2*pi)^n*det(Sigma)) If q(mu; M, Z) = exp(-error) & k = 1.0 / sqrt((2*pi)^n*det(Sigma)) - thus, q*sqrt(|2*pi*Sigma|) = q/k = exp(log(q/k)) + thus, q * sqrt((2*pi)^n*det(Sigma)) = q/k = exp(log(q/k)) = exp(log(q) - log(k)) = exp(-error - log(k)) = exp(-(error + log(k))) - So let's compute (error + log(k)) and exponentiate later + So we compute (error + log(k)) and exponentiate later */ - error = error + gm->error(continuousValues); - // Add the logNormalization constant to the error + // Add the error and the logNormalization constant to the error + auto err = gm->error(continuousValues) + gm->logNormalizationConstant(); + // Also compute the sum for discrete probability normalization // (normalization trick for numerical stability) double sum = 0.0; - auto addConstant = [&gm, &sum](const double &error) { - double e = error + gm->logNormalizationConstant(); + auto absSum = [&sum](const double &e) { sum += std::abs(e); return e; }; - error = error.apply(addConstant); - // Normalize by the sum - error = error.normalize(sum); + err.visit(absSum); + // Normalize by the sum to prevent overflow + error = error + err.normalize(sum); // Include the discrete keys std::copy(gm->discreteKeys().begin(), gm->discreteKeys().end(), @@ -302,11 +302,11 @@ HybridValues HybridBayesNet::optimize() const { } } - double min_log = error.min(); - AlgebraicDecisionTree model_selection = - DecisionTree(error, [&min_log](const double &x) { - return std::exp(-(x - min_log)) * exp(-min_log); - }); + error = error * -1; + double max_log = error.max(); + AlgebraicDecisionTree model_selection = DecisionTree( + error, [&max_log](const double &x) { return std::exp(x - max_log); }); + model_selection = model_selection.normalize(model_selection.sum()); // Only add model_selection if we have discrete keys if (discreteKeySet.size() > 0) { diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index dfff3d4f3..260dc6bbe 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -328,7 +328,6 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // The residual error contains no keys, and only depends on the discrete // separator if present. auto logProbability = [&](const Result &pair) -> double { - // auto probability = [&](const Result &pair) -> double { static const VectorValues kEmpty; // If the factor is not null, it has no keys, just contains the residual. const auto &factor = pair.second; @@ -343,9 +342,11 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // Perform normalization double max_log = logProbabilities.max(); - DecisionTree probabilities( + AlgebraicDecisionTree probabilities = DecisionTree( logProbabilities, - [&max_log](const double x) { return exp(x - max_log) * exp(max_log); }); + [&max_log](const double x) { return exp(x - max_log); }); + // probabilities.print("", DefaultKeyFormatter); + probabilities = probabilities.normalize(probabilities.sum()); return { std::make_shared(gaussianMixture), diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index b8edc39d8..1cc28b386 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -333,7 +333,6 @@ TEST(HybridEstimation, Probability) { for (auto discrete_conditional : *discreteBayesNet) { bayesNet->add(discrete_conditional); } - auto discreteConditional = discreteBayesNet->at(0)->asDiscrete(); HybridValues hybrid_values = bayesNet->optimize(); diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 0112835aa..4ec1d8b95 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -184,8 +184,10 @@ namespace gtsam { double GaussianConditional::logNormalizationConstant() const { constexpr double log2pi = 1.8378770664093454835606594728112; size_t n = d().size(); - // log det(Sigma)) = - 2.0 * logDeterminant() - return - 0.5 * n * log2pi + logDeterminant(); + // Sigma = (R'R)^{-1}, det(Sigma) = det((R'R)^{-1}) = det(R'R)^{-1} + // log det(Sigma) = -log(det(R'R)) = -2*log(det(R)) + // Hence, log det(Sigma)) = - 2.0 * logDeterminant() + return -0.5 * n * log2pi + logDeterminant(); } /* ************************************************************************* */ From 07ddec5f4b4aac614bc1730022b130c99b59a944 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 21 Dec 2023 14:19:21 -0500 Subject: [PATCH 12/71] remove stray comment --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 260dc6bbe..d63f8b5ff 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -345,7 +345,6 @@ hybridElimination(const HybridGaussianFactorGraph &factors, AlgebraicDecisionTree probabilities = DecisionTree( logProbabilities, [&max_log](const double x) { return exp(x - max_log); }); - // probabilities.print("", DefaultKeyFormatter); probabilities = probabilities.normalize(probabilities.sum()); return { From c6584f63cea0f2638d2a04a76573cf971ac75c2a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 22 Dec 2023 14:55:31 -0500 Subject: [PATCH 13/71] minor test cleanup --- gtsam/hybrid/tests/testMixtureFactor.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp index 4bb716540..4819458e8 100644 --- a/gtsam/hybrid/tests/testMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testMixtureFactor.cpp @@ -202,20 +202,19 @@ TEST(MixtureFactor, DifferentCovariances) { {Z(1)}, {X(1), X(2)}, {m1}, {std::make_shared(terms0, 1, -d0, model0), std::make_shared(terms1, 1, -d1, model1)}); - gtsam::HybridBayesNet bn2; - bn2.emplace_back(gm); + gtsam::HybridBayesNet bn; + bn.emplace_back(gm); gtsam::VectorValues measurements; measurements.insert(Z(1), gtsam::Z_1x1); // Create FG with single GaussianMixtureFactor - auto mixture_fg = bn2.toFactorGraph(measurements); + HybridGaussianFactorGraph mixture_fg = bn.toFactorGraph(measurements); // Linearized prior factor on X1 auto prior = PriorFactor(X(1), x1, prior_noise).linearize(values); mixture_fg.push_back(prior); auto hbn = mixture_fg.eliminateSequential(); - // hbn->print("\n\nfinal bayes net"); HybridValues actual_values = hbn->optimize(); From ebcf958d691f1be867900bb5c0a45b24ad36eff9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 25 Dec 2023 16:04:48 -0500 Subject: [PATCH 14/71] better, more correct version of model selection --- gtsam/hybrid/HybridBayesNet.cpp | 208 ++++++++++++++++++++++---------- 1 file changed, 142 insertions(+), 66 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 2b0b11e36..23a1c7787 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -26,6 +26,18 @@ static std::mt19937_64 kRandomNumberGenerator(42); namespace gtsam { +using std::dynamic_pointer_cast; + +/* ************************************************************************ */ +// Throw a runtime exception for method specified in string s, +// and conditional f: +static void throwRuntimeError(const std::string &s, + const std::shared_ptr &f) { + auto &fr = *f; + throw std::runtime_error(s + " not implemented for conditional type " + + demangle(typeid(fr).name()) + "."); +} + /* ************************************************************************* */ void HybridBayesNet::print(const std::string &s, const KeyFormatter &formatter) const { @@ -217,6 +229,56 @@ GaussianBayesNet HybridBayesNet::choose( return gbn; } +/* ************************************************************************ */ +static GaussianBayesNetTree addGaussian( + const GaussianBayesNetTree &gfgTree, + const GaussianConditional::shared_ptr &factor) { + // If the decision tree is not initialized, then initialize it. + if (gfgTree.empty()) { + GaussianBayesNet result{factor}; + return GaussianBayesNetTree(result); + } else { + auto add = [&factor](const GaussianBayesNet &graph) { + auto result = graph; + result.push_back(factor); + return result; + }; + return gfgTree.apply(add); + } +} + +/* ************************************************************************ */ +GaussianBayesNetTree HybridBayesNet::assembleTree() const { + GaussianBayesNetTree result; + + for (auto &f : factors_) { + // TODO(dellaert): just use a virtual method defined in HybridFactor. + if (auto gm = dynamic_pointer_cast(f)) { + result = gm->add(result); + } else if (auto hc = dynamic_pointer_cast(f)) { + if (auto gm = hc->asMixture()) { + result = gm->add(result); + } else if (auto g = hc->asGaussian()) { + result = addGaussian(result, g); + } else { + // Has to be discrete. + // TODO(dellaert): in C++20, we can use std::visit. + continue; + } + } else if (dynamic_pointer_cast(f)) { + // Don't do anything for discrete-only factors + // since we want to evaluate continuous values only. + continue; + } else { + // We need to handle the case where the object is actually an + // BayesTreeOrphanWrapper! + throwRuntimeError("HybridBayesNet::assembleTree", f); + } + } + + return result; +} + /* ************************************************************************* */ HybridValues HybridBayesNet::optimize() const { // Collect all the discrete factors to compute MPE @@ -227,74 +289,94 @@ HybridValues HybridBayesNet::optimize() const { AlgebraicDecisionTree error(0.0); std::set discreteKeySet; + // this->print(); + GaussianBayesNetTree bnTree = assembleTree(); + // bnTree.print("", DefaultKeyFormatter, [](const GaussianBayesNet &gbn) { + // gbn.print(); + // return ""; + // }); + /* + Perform the integration of L(X;M,Z)P(X|M) + which is the model selection term. + + By Bayes' rule, P(X|M,Z) ∝ L(X;M,Z)P(X|M), + hence L(X;M,Z)P(X|M) is the unnormalized probabilty of + the joint Gaussian distribution. + + This can be computed by multiplying all the exponentiated errors + of each of the conditionals, which we do below in hybrid case. + */ + /* + To perform model selection, we need: + q(mu; M, Z) * sqrt((2*pi)^n*det(Sigma)) + + If q(mu; M, Z) = exp(-error) & k = 1.0 / sqrt((2*pi)^n*det(Sigma)) + thus, q * sqrt((2*pi)^n*det(Sigma)) = q/k = exp(log(q/k)) + = exp(log(q) - log(k)) = exp(-error - log(k)) + = exp(-(error + log(k))) + + So we compute (error + log(k)) and exponentiate later + */ + // Compute the X* of each assignment and use that as the MAP. + DecisionTree x_map( + bnTree, [](const GaussianBayesNet &gbn) { return gbn.optimize(); }); + + // Only compute logNormalizationConstant for now + AlgebraicDecisionTree log_norm_constants = + DecisionTree(bnTree, [](const GaussianBayesNet &gbn) { + if (gbn.size() == 0) { + return -std::numeric_limits::max(); + } + return -gbn.logNormalizationConstant(); + }); + // Compute unnormalized error term and compute model selection term + AlgebraicDecisionTree model_selection_term = log_norm_constants.apply( + [this, &x_map](const Assignment &assignment, double x) { + double error = 0.0; + for (auto &&f : *this) { + if (auto gm = dynamic_pointer_cast(f)) { + error += gm->error( + HybridValues(x_map(assignment), DiscreteValues(assignment))); + } else if (auto hc = dynamic_pointer_cast(f)) { + if (auto gm = hc->asMixture()) { + error += gm->error( + HybridValues(x_map(assignment), DiscreteValues(assignment))); + } else if (auto g = hc->asGaussian()) { + error += g->error(x_map(assignment)); + } + } + } + return -(error + x); + }); + // model_selection_term.print("", DefaultKeyFormatter); + + double max_log = model_selection_term.max(); + AlgebraicDecisionTree model_selection = DecisionTree( + model_selection_term, + [&max_log](const double &x) { return std::exp(x - max_log); }); + model_selection = model_selection.normalize(model_selection.sum()); + // std::cout << "normalized model selection" << std::endl; + // model_selection.print("", DefaultKeyFormatter); + for (auto &&conditional : *this) { if (conditional->isDiscrete()) { discrete_fg.push_back(conditional->asDiscrete()); } else { - /* - Perform the integration of L(X;M,Z)P(X|M) - which is the model selection term. - - By Bayes' rule, P(X|M) ∝ L(X;M,Z)P(X|M), - hence L(X;M,Z)P(X|M) is the unnormalized probabilty of - the joint Gaussian distribution. - - This can be computed by multiplying all the exponentiated errors - of each of the conditionals, which we do below in hybrid case. - */ if (conditional->isContinuous()) { - /* - If we are here, it means there are no discrete variables in - the Bayes net (due to strong elimination ordering). - This is a continuous-only problem hence model selection doesn't matter. - */ - auto gc = conditional->asGaussian(); - for (GaussianConditional::const_iterator frontal = gc->beginFrontals(); - frontal != gc->endFrontals(); ++frontal) { - continuousValues.insert_or_assign(*frontal, - Vector::Zero(gc->getDim(frontal))); - } + // /* + // If we are here, it means there are no discrete variables in + // the Bayes net (due to strong elimination ordering). + // This is a continuous-only problem hence model selection doesn't matter. + // */ + // auto gc = conditional->asGaussian(); + // for (GaussianConditional::const_iterator frontal = gc->beginFrontals(); + // frontal != gc->endFrontals(); ++frontal) { + // continuousValues.insert_or_assign(*frontal, + // Vector::Zero(gc->getDim(frontal))); + // } } else if (conditional->isHybrid()) { auto gm = conditional->asMixture(); - gm->conditionals().apply( - [&continuousValues](const GaussianConditional::shared_ptr &gc) { - if (gc) { - for (GaussianConditional::const_iterator frontal = gc->begin(); - frontal != gc->end(); ++frontal) { - continuousValues.insert_or_assign( - *frontal, Vector::Zero(gc->getDim(frontal))); - } - } - return gc; - }); - - /* - To perform model selection, we need: - q(mu; M, Z) * sqrt((2*pi)^n*det(Sigma)) - - If q(mu; M, Z) = exp(-error) & k = 1.0 / sqrt((2*pi)^n*det(Sigma)) - thus, q * sqrt((2*pi)^n*det(Sigma)) = q/k = exp(log(q/k)) - = exp(log(q) - log(k)) = exp(-error - log(k)) - = exp(-(error + log(k))) - - So we compute (error + log(k)) and exponentiate later - */ - - // Add the error and the logNormalization constant to the error - auto err = gm->error(continuousValues) + gm->logNormalizationConstant(); - - // Also compute the sum for discrete probability normalization - // (normalization trick for numerical stability) - double sum = 0.0; - auto absSum = [&sum](const double &e) { - sum += std::abs(e); - return e; - }; - err.visit(absSum); - // Normalize by the sum to prevent overflow - error = error + err.normalize(sum); - // Include the discrete keys std::copy(gm->discreteKeys().begin(), gm->discreteKeys().end(), std::inserter(discreteKeySet, discreteKeySet.end())); @@ -302,12 +384,6 @@ HybridValues HybridBayesNet::optimize() const { } } - error = error * -1; - double max_log = error.max(); - AlgebraicDecisionTree model_selection = DecisionTree( - error, [&max_log](const double &x) { return std::exp(x - max_log); }); - model_selection = model_selection.normalize(model_selection.sum()); - // Only add model_selection if we have discrete keys if (discreteKeySet.size() > 0) { discrete_fg.push_back(DecisionTreeFactor( From 1e298be3b3b09dbb996d40c8d2b7d0997f024508 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 25 Dec 2023 18:47:44 -0500 Subject: [PATCH 15/71] Better way of handling assignments --- gtsam/hybrid/HybridBayesNet.cpp | 76 +++++++++++++++++++-------------- 1 file changed, 43 insertions(+), 33 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 23a1c7787..89be86056 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -285,8 +285,6 @@ HybridValues HybridBayesNet::optimize() const { DiscreteFactorGraph discrete_fg; VectorValues continuousValues; - // Error values for each hybrid factor - AlgebraicDecisionTree error(0.0); std::set discreteKeySet; // this->print(); @@ -313,7 +311,8 @@ HybridValues HybridBayesNet::optimize() const { If q(mu; M, Z) = exp(-error) & k = 1.0 / sqrt((2*pi)^n*det(Sigma)) thus, q * sqrt((2*pi)^n*det(Sigma)) = q/k = exp(log(q/k)) = exp(log(q) - log(k)) = exp(-error - log(k)) - = exp(-(error + log(k))) + = exp(-(error + log(k))), + where error is computed at the corresponding MAP point, gbn.error(mu). So we compute (error + log(k)) and exponentiate later */ @@ -325,29 +324,45 @@ HybridValues HybridBayesNet::optimize() const { AlgebraicDecisionTree log_norm_constants = DecisionTree(bnTree, [](const GaussianBayesNet &gbn) { if (gbn.size() == 0) { - return -std::numeric_limits::max(); + return 0.0; } - return -gbn.logNormalizationConstant(); + return gbn.logNormalizationConstant(); }); - // Compute unnormalized error term and compute model selection term - AlgebraicDecisionTree model_selection_term = log_norm_constants.apply( - [this, &x_map](const Assignment &assignment, double x) { - double error = 0.0; - for (auto &&f : *this) { - if (auto gm = dynamic_pointer_cast(f)) { - error += gm->error( - HybridValues(x_map(assignment), DiscreteValues(assignment))); - } else if (auto hc = dynamic_pointer_cast(f)) { - if (auto gm = hc->asMixture()) { - error += gm->error( - HybridValues(x_map(assignment), DiscreteValues(assignment))); - } else if (auto g = hc->asGaussian()) { - error += g->error(x_map(assignment)); - } - } + + // Compute unnormalized error term + std::vector labels; + for (auto &&key : x_map.labels()) { + labels.push_back(std::make_pair(key, 2)); + } + + std::vector errors; + x_map.visitWith([this, &errors](const Assignment &assignment, + const VectorValues &mu) { + double error = 0.0; + for (auto &&f : *this) { + if (auto gm = dynamic_pointer_cast(f)) { + error += gm->error(HybridValues(mu, DiscreteValues(assignment))); + } else if (auto hc = dynamic_pointer_cast(f)) { + if (auto gm = hc->asMixture()) { + error += gm->error(HybridValues(mu, DiscreteValues(assignment))); + } else if (auto g = hc->asGaussian()) { + error += g->error(mu); } - return -(error + x); + } + } + errors.push_back(error); + }); + + AlgebraicDecisionTree errorTree = + DecisionTree(labels, errors); + + // Compute model selection term + AlgebraicDecisionTree model_selection_term = errorTree.apply( + [&log_norm_constants](const Assignment assignment, double err) { + return -(err + log_norm_constants(assignment)); }); + + // std::cout << "model selection term" << std::endl; // model_selection_term.print("", DefaultKeyFormatter); double max_log = model_selection_term.max(); @@ -355,6 +370,7 @@ HybridValues HybridBayesNet::optimize() const { model_selection_term, [&max_log](const double &x) { return std::exp(x - max_log); }); model_selection = model_selection.normalize(model_selection.sum()); + // std::cout << "normalized model selection" << std::endl; // model_selection.print("", DefaultKeyFormatter); @@ -363,17 +379,11 @@ HybridValues HybridBayesNet::optimize() const { discrete_fg.push_back(conditional->asDiscrete()); } else { if (conditional->isContinuous()) { - // /* - // If we are here, it means there are no discrete variables in - // the Bayes net (due to strong elimination ordering). - // This is a continuous-only problem hence model selection doesn't matter. - // */ - // auto gc = conditional->asGaussian(); - // for (GaussianConditional::const_iterator frontal = gc->beginFrontals(); - // frontal != gc->endFrontals(); ++frontal) { - // continuousValues.insert_or_assign(*frontal, - // Vector::Zero(gc->getDim(frontal))); - // } + /* + If we are here, it means there are no discrete variables in + the Bayes net (due to strong elimination ordering). + This is a continuous-only problem hence model selection doesn't matter. + */ } else if (conditional->isHybrid()) { auto gm = conditional->asMixture(); From b4f07a01629f5ccb8d9413a82c9b9453627be28d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 25 Dec 2023 23:11:49 -0500 Subject: [PATCH 16/71] cleaner model selection computation --- gtsam/hybrid/HybridBayesNet.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 89be86056..90951b074 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -342,9 +342,11 @@ HybridValues HybridBayesNet::optimize() const { for (auto &&f : *this) { if (auto gm = dynamic_pointer_cast(f)) { error += gm->error(HybridValues(mu, DiscreteValues(assignment))); + } else if (auto hc = dynamic_pointer_cast(f)) { if (auto gm = hc->asMixture()) { error += gm->error(HybridValues(mu, DiscreteValues(assignment))); + } else if (auto g = hc->asGaussian()) { error += g->error(mu); } @@ -356,11 +358,9 @@ HybridValues HybridBayesNet::optimize() const { AlgebraicDecisionTree errorTree = DecisionTree(labels, errors); - // Compute model selection term - AlgebraicDecisionTree model_selection_term = errorTree.apply( - [&log_norm_constants](const Assignment assignment, double err) { - return -(err + log_norm_constants(assignment)); - }); + // Compute model selection term (with help from ADT methods) + AlgebraicDecisionTree model_selection_term = + (errorTree + log_norm_constants) * -1; // std::cout << "model selection term" << std::endl; // model_selection_term.print("", DefaultKeyFormatter); From 6f4343ca94aba06aff18fb3c019d18860af8f219 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 26 Dec 2023 00:20:44 -0500 Subject: [PATCH 17/71] almost working --- gtsam/hybrid/HybridBayesNet.cpp | 45 +++++++++++++++------------------ 1 file changed, 20 insertions(+), 25 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 90951b074..0ff4e342b 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -329,34 +329,29 @@ HybridValues HybridBayesNet::optimize() const { return gbn.logNormalizationConstant(); }); - // Compute unnormalized error term - std::vector labels; - for (auto &&key : x_map.labels()) { - labels.push_back(std::make_pair(key, 2)); - } + // Compute errors as VectorValues + DecisionTree errorVectors = x_map.apply( + [this](const Assignment &assignment, const VectorValues &mu) { + double error = 0.0; + for (auto &&f : *this) { + if (auto gm = dynamic_pointer_cast(f)) { + error += gm->error(HybridValues(mu, DiscreteValues(assignment))); - std::vector errors; - x_map.visitWith([this, &errors](const Assignment &assignment, - const VectorValues &mu) { - double error = 0.0; - for (auto &&f : *this) { - if (auto gm = dynamic_pointer_cast(f)) { - error += gm->error(HybridValues(mu, DiscreteValues(assignment))); + } else if (auto hc = dynamic_pointer_cast(f)) { + if (auto gm = hc->asMixture()) { + error += gm->error(HybridValues(mu, DiscreteValues(assignment))); - } else if (auto hc = dynamic_pointer_cast(f)) { - if (auto gm = hc->asMixture()) { - error += gm->error(HybridValues(mu, DiscreteValues(assignment))); - - } else if (auto g = hc->asGaussian()) { - error += g->error(mu); + } else if (auto g = hc->asGaussian()) { + error += g->error(mu); + } + } } - } - } - errors.push_back(error); - }); - - AlgebraicDecisionTree errorTree = - DecisionTree(labels, errors); + VectorValues e; + e.insert(0, Vector1(error)); + return e; + }); + AlgebraicDecisionTree errorTree = DecisionTree( + errorVectors, [](const VectorValues &v) { return v[0](0); }); // Compute model selection term (with help from ADT methods) AlgebraicDecisionTree model_selection_term = From 409938f4b493fde3c2f7fe84c6ba3b914a115d5a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 26 Dec 2023 16:33:43 -0500 Subject: [PATCH 18/71] improved model selection code --- gtsam/hybrid/HybridBayesNet.cpp | 68 +++++++++++++-------------------- gtsam/hybrid/HybridBayesNet.h | 2 + gtsam/hybrid/HybridFactor.h | 8 ++++ 3 files changed, 37 insertions(+), 41 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 0ff4e342b..027bd75d4 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -248,7 +248,7 @@ static GaussianBayesNetTree addGaussian( } /* ************************************************************************ */ -GaussianBayesNetTree HybridBayesNet::assembleTree() const { +GaussianBayesNetValTree HybridBayesNet::assembleTree() const { GaussianBayesNetTree result; for (auto &f : factors_) { @@ -276,23 +276,17 @@ GaussianBayesNetTree HybridBayesNet::assembleTree() const { } } - return result; + GaussianBayesNetValTree resultTree(result, [](const GaussianBayesNet &gbn) { + return std::make_pair(gbn, 0.0); + }); + return resultTree; } /* ************************************************************************* */ HybridValues HybridBayesNet::optimize() const { // Collect all the discrete factors to compute MPE DiscreteFactorGraph discrete_fg; - VectorValues continuousValues; - std::set discreteKeySet; - - // this->print(); - GaussianBayesNetTree bnTree = assembleTree(); - // bnTree.print("", DefaultKeyFormatter, [](const GaussianBayesNet &gbn) { - // gbn.print(); - // return ""; - // }); /* Perform the integration of L(X;M,Z)P(X|M) which is the model selection term. @@ -316,43 +310,35 @@ HybridValues HybridBayesNet::optimize() const { So we compute (error + log(k)) and exponentiate later */ - // Compute the X* of each assignment and use that as the MAP. - DecisionTree x_map( - bnTree, [](const GaussianBayesNet &gbn) { return gbn.optimize(); }); - // Only compute logNormalizationConstant for now - AlgebraicDecisionTree log_norm_constants = - DecisionTree(bnTree, [](const GaussianBayesNet &gbn) { + std::set discreteKeySet; + GaussianBayesNetValTree bnTree = assembleTree(); + + GaussianBayesNetValTree bn_error = bnTree.apply( + [this](const Assignment &assignment, + const std::pair &gbnAndValue) { + // Compute the X* of each assignment + VectorValues mu = gbnAndValue.first.optimize(); + // Compute the error for X* and the assignment + double error = + this->error(HybridValues(mu, DiscreteValues(assignment))); + + return std::make_pair(gbnAndValue.first, error); + }); + + auto trees = unzip(bn_error); + AlgebraicDecisionTree errorTree = trees.second; + + // Only compute logNormalizationConstant + AlgebraicDecisionTree log_norm_constants = DecisionTree( + bnTree, [](const std::pair &gbnAndValue) { + GaussianBayesNet gbn = gbnAndValue.first; if (gbn.size() == 0) { return 0.0; } return gbn.logNormalizationConstant(); }); - // Compute errors as VectorValues - DecisionTree errorVectors = x_map.apply( - [this](const Assignment &assignment, const VectorValues &mu) { - double error = 0.0; - for (auto &&f : *this) { - if (auto gm = dynamic_pointer_cast(f)) { - error += gm->error(HybridValues(mu, DiscreteValues(assignment))); - - } else if (auto hc = dynamic_pointer_cast(f)) { - if (auto gm = hc->asMixture()) { - error += gm->error(HybridValues(mu, DiscreteValues(assignment))); - - } else if (auto g = hc->asGaussian()) { - error += g->error(mu); - } - } - } - VectorValues e; - e.insert(0, Vector1(error)); - return e; - }); - AlgebraicDecisionTree errorTree = DecisionTree( - errorVectors, [](const VectorValues &v) { return v[0](0); }); - // Compute model selection term (with help from ADT methods) AlgebraicDecisionTree model_selection_term = (errorTree + log_norm_constants) * -1; diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 22e03bba9..f0cdbaaf9 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -118,6 +118,8 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { return evaluate(values); } + GaussianBayesNetValTree assembleTree() const; + /** * @brief Solve the HybridBayesNet by first computing the MPE of all the * discrete variables and then optimizing the continuous variables based on diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index afd1c8032..8828a9172 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -33,6 +33,14 @@ class HybridValues; /// Alias for DecisionTree of GaussianFactorGraphs using GaussianFactorGraphTree = DecisionTree; +/// Alias for DecisionTree of GaussianBayesNets +using GaussianBayesNetTree = DecisionTree; +/** + * Alias for DecisionTree of (GaussianBayesNet, double) pairs. + * Used for model selection in BayesNet::optimize + */ +using GaussianBayesNetValTree = + DecisionTree>; KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys); From 93c824c482babdb48cb7c856d906757c57841d3f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 27 Dec 2023 15:34:25 -0500 Subject: [PATCH 19/71] overload == operator for GaussianBayesNet and VectorValues --- gtsam/linear/GaussianBayesNet.h | 6 ++++++ gtsam/linear/VectorValues.h | 5 +++++ 2 files changed, 11 insertions(+) diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 19781d1e6..d4d60845d 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -82,6 +82,12 @@ namespace gtsam { /** Check equality */ bool equals(const This& bn, double tol = 1e-9) const; + /// Check exact equality. + friend bool operator==(const GaussianBayesNet& lhs, + const GaussianBayesNet& rhs) { + return lhs.isEqual(rhs); + } + /// print graph void print( const std::string& s = "", diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 478cfd770..865fa3c8a 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -260,6 +260,11 @@ namespace gtsam { /** equals required by Testable for unit testing */ bool equals(const VectorValues& x, double tol = 1e-9) const; + /// Check exact equality. + friend bool operator==(const VectorValues& lhs, const VectorValues& rhs) { + return lhs.equals(rhs); + } + /// @{ /// @name Advanced Interface /// @{ From b20d33d79e8aaac6c0cbc76dcee4ba35180c93cb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 27 Dec 2023 15:38:30 -0500 Subject: [PATCH 20/71] logNormalizationConstant() for GaussianBayesNet --- gtsam/linear/GaussianBayesNet.cpp | 20 ++++++++++++++++++++ gtsam/linear/GaussianBayesNet.h | 8 ++++++++ 2 files changed, 28 insertions(+) diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index b04878ac5..861e19cc9 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -243,5 +243,25 @@ namespace gtsam { } /* ************************************************************************* */ + double GaussianBayesNet::logNormalizationConstant() const { + /* + normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma)) + logConstant = -0.5 * n*log(2*pi) - 0.5 * log det(Sigma) + + log det(Sigma)) = -2.0 * logDeterminant() + thus, logConstant = -0.5*n*log(2*pi) + logDeterminant() + + BayesNet logConstant = sum(-0.5*n_i*log(2*pi) + logDeterminant_i()) + = sum(-0.5*n_i*log(2*pi)) + sum(logDeterminant_i()) + = sum(-0.5*n_i*log(2*pi)) + bn->logDeterminant() + */ + double logNormConst = 0.0; + for (const sharedConditional& cg : *this) { + logNormConst += cg->logNormalizationConstant(); + } + return logNormConst; + } + + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index d4d60845d..ea1cb8603 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -234,6 +234,14 @@ namespace gtsam { * @return The determinant */ double logDeterminant() const; + /** + * @brief Get the log of the normalization constant corresponding to the + * joint Gaussian density represented by this Bayes net. + * + * @return double + */ + double logNormalizationConstant() const; + /** * Backsubstitute with a different RHS vector than the one stored in this BayesNet. * gy=inv(R*inv(Sigma))*gx From 3a89653e91b9c4fd09d047fef860bf5c3f9fb794 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 27 Dec 2023 15:45:35 -0500 Subject: [PATCH 21/71] helper methods in GaussianMixture for model selection --- gtsam/hybrid/GaussianMixture.cpp | 55 ++++++++++++++++++++++++++++++-- gtsam/hybrid/GaussianMixture.h | 15 +++++++++ 2 files changed, 68 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index a1ba167f5..61b40e566 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include namespace gtsam { @@ -92,6 +93,34 @@ GaussianFactorGraphTree GaussianMixture::asGaussianFactorGraphTree() const { return {conditionals_, wrap}; } +/* *******************************************************************************/ +GaussianBayesNetTree GaussianMixture::add( + const GaussianBayesNetTree &sum) const { + using Y = GaussianBayesNet; + auto add = [](const Y &graph1, const Y &graph2) { + auto result = graph1; + if (graph2.size() == 0) { + return GaussianBayesNet(); + } + result.push_back(graph2); + return result; + }; + const auto tree = asGaussianBayesNetTree(); + return sum.empty() ? tree : sum.apply(tree, add); +} + +/* *******************************************************************************/ +GaussianBayesNetTree GaussianMixture::asGaussianBayesNetTree() const { + auto wrap = [](const GaussianConditional::shared_ptr &gc) { + if (gc) { + return GaussianBayesNet{gc}; + } else { + return GaussianBayesNet(); + } + }; + return {conditionals_, wrap}; +} + /* *******************************************************************************/ size_t GaussianMixture::nrComponents() const { size_t total = 0; @@ -332,10 +361,32 @@ AlgebraicDecisionTree GaussianMixture::error( /* *******************************************************************************/ double GaussianMixture::error(const HybridValues &values) const { + // Check if discrete keys in discrete assignment are + // present in the GaussianMixture + KeyVector dKeys = this->discreteKeys_.indices(); + bool valid_assignment = false; + for (auto &&kv : values.discrete()) { + if (std::find(dKeys.begin(), dKeys.end(), kv.first) != dKeys.end()) { + valid_assignment = true; + break; + } + } + + // The discrete assignment is not valid so we return 0.0 erorr. + if (!valid_assignment) { + return 0.0; + } + // Directly index to get the conditional, no need to build the whole tree. auto conditional = conditionals_(values.discrete()); - return conditional->error(values.continuous()) + // - logConstant_ - conditional->logNormalizationConstant(); + if (conditional) { + return conditional->error(values.continuous()) + // + logConstant_ - conditional->logNormalizationConstant(); + } else { + // If not valid, pointer, it means this conditional was pruned, + // so we return maximum error. + return std::numeric_limits::max(); + } } /* *******************************************************************************/ diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index 0b68fcfd0..a52cd6c82 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -71,6 +71,12 @@ class GTSAM_EXPORT GaussianMixture */ GaussianFactorGraphTree asGaussianFactorGraphTree() const; + /** + * @brief Convert a DecisionTree of conditionals into + * a DT of Gaussian Bayes nets. + */ + GaussianBayesNetTree asGaussianBayesNetTree() const; + /** * @brief Helper function to get the pruner functor. * @@ -248,6 +254,15 @@ class GTSAM_EXPORT GaussianMixture * @return GaussianFactorGraphTree */ GaussianFactorGraphTree add(const GaussianFactorGraphTree &sum) const; + + /** + * @brief Merge the Gaussian Bayes Nets in `this` and `sum` while + * maintaining the decision tree structure. + * + * @param sum Decision Tree of Gaussian Bayes Nets + * @return GaussianBayesNetTree + */ + GaussianBayesNetTree add(const GaussianBayesNetTree &sum) const; /// @} private: From 6f66d04f1425219f51b284c4b4094a15dc2a5791 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 27 Dec 2023 15:46:31 -0500 Subject: [PATCH 22/71] handle pruning in model selection --- gtsam/hybrid/HybridBayesNet.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 027bd75d4..0352d7962 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -319,6 +319,13 @@ HybridValues HybridBayesNet::optimize() const { const std::pair &gbnAndValue) { // Compute the X* of each assignment VectorValues mu = gbnAndValue.first.optimize(); + + // mu is empty if gbn had nullptrs + if (mu.size() == 0) { + return std::make_pair(gbnAndValue.first, + std::numeric_limits::max()); + } + // Compute the error for X* and the assignment double error = this->error(HybridValues(mu, DiscreteValues(assignment))); @@ -343,18 +350,12 @@ HybridValues HybridBayesNet::optimize() const { AlgebraicDecisionTree model_selection_term = (errorTree + log_norm_constants) * -1; - // std::cout << "model selection term" << std::endl; - // model_selection_term.print("", DefaultKeyFormatter); - double max_log = model_selection_term.max(); AlgebraicDecisionTree model_selection = DecisionTree( model_selection_term, [&max_log](const double &x) { return std::exp(x - max_log); }); model_selection = model_selection.normalize(model_selection.sum()); - // std::cout << "normalized model selection" << std::endl; - // model_selection.print("", DefaultKeyFormatter); - for (auto &&conditional : *this) { if (conditional->isDiscrete()) { discrete_fg.push_back(conditional->asDiscrete()); From 0d058100e57d187de3b7c4638addf21da1c8158b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 3 Jan 2024 14:44:12 -0500 Subject: [PATCH 23/71] update wrapper for LM with Ordering parameter --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 2 ++ gtsam/nonlinear/nonlinear.i | 11 ++++++++--- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index d63f8b5ff..522dc626b 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -333,6 +333,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, const auto &factor = pair.second; if (!factor) return 1.0; // TODO(dellaert): not loving this. + // Logspace version of: // exp(-factor->error(kEmpty)) / pair.first->normalizationConstant(); return -factor->error(kEmpty) - pair.first->logNormalizationConstant(); }; @@ -345,6 +346,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, AlgebraicDecisionTree probabilities = DecisionTree( logProbabilities, [&max_log](const double x) { return exp(x - max_log); }); + // probabilities.print("", DefaultKeyFormatter); probabilities = probabilities.normalize(probabilities.sum()); return { diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 3f5fb1dd5..113f08e0f 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -378,17 +378,22 @@ typedef gtsam::GncOptimizer> G #include virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& initialValues); + const gtsam::Values& initialValues, + const gtsam::LevenbergMarquardtParams& params = + gtsam::LevenbergMarquardtParams()); LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, - const gtsam::LevenbergMarquardtParams& params); + const gtsam::Ordering& ordering, + const gtsam::LevenbergMarquardtParams& params = + gtsam::LevenbergMarquardtParams()); + double lambda() const; void print(string s = "") const; }; #include class ISAM2GaussNewtonParams { - ISAM2GaussNewtonParams(); + ISAM2GaussNewtonParams(double _wildfireThreshold = 0.001); void print(string s = "") const; From 651f99925b98a0bd5cb531927af3f3ef7f9c6af2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 3 Jan 2024 16:13:02 -0500 Subject: [PATCH 24/71] print logNormalizationConstant for Gaussian conditionals --- gtsam/hybrid/GaussianMixture.cpp | 2 ++ .../tests/testHybridNonlinearFactorGraph.cpp | 16 ++++++++++++++++ gtsam/linear/GaussianConditional.cpp | 1 + gtsam/linear/tests/testGaussianConditional.cpp | 3 +++ 4 files changed, 22 insertions(+) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 61b40e566..b7840429b 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -174,6 +174,8 @@ void GaussianMixture::print(const std::string &s, std::cout << "(" << formatter(dk.first) << ", " << dk.second << "), "; } std::cout << "\n"; + std::cout << " logNormalizationConstant: " << logConstant_ << "\n" + << std::endl; conditionals_.print( "", [&](Key k) { return formatter(k); }, [&](const GaussianConditional::shared_ptr &gf) -> std::string { diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 93081d309..1dcd0bbb1 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -675,33 +675,41 @@ factor 6: P( m1 | m0 ): size: 3 conditional 0: Hybrid P( x0 | x1 m0) Discrete Keys = (m0, 2), + logNormalizationConstant: 1.38862 + Choice(m0) 0 Leaf p(x0 | x1) R = [ 10.0499 ] S[x1] = [ -0.0995037 ] d = [ -9.85087 ] + logNormalizationConstant: 1.38862 No noise model 1 Leaf p(x0 | x1) R = [ 10.0499 ] S[x1] = [ -0.0995037 ] d = [ -9.95037 ] + logNormalizationConstant: 1.38862 No noise model conditional 1: Hybrid P( x1 | x2 m0 m1) Discrete Keys = (m0, 2), (m1, 2), + logNormalizationConstant: 1.3935 + Choice(m1) 0 Choice(m0) 0 0 Leaf p(x1 | x2) R = [ 10.099 ] S[x2] = [ -0.0990196 ] d = [ -9.99901 ] + logNormalizationConstant: 1.3935 No noise model 0 1 Leaf p(x1 | x2) R = [ 10.099 ] S[x2] = [ -0.0990196 ] d = [ -9.90098 ] + logNormalizationConstant: 1.3935 No noise model 1 Choice(m0) @@ -709,16 +717,20 @@ conditional 1: Hybrid P( x1 | x2 m0 m1) R = [ 10.099 ] S[x2] = [ -0.0990196 ] d = [ -10.098 ] + logNormalizationConstant: 1.3935 No noise model 1 1 Leaf p(x1 | x2) R = [ 10.099 ] S[x2] = [ -0.0990196 ] d = [ -10 ] + logNormalizationConstant: 1.3935 No noise model conditional 2: Hybrid P( x2 | m0 m1) Discrete Keys = (m0, 2), (m1, 2), + logNormalizationConstant: 1.38857 + Choice(m1) 0 Choice(m0) 0 0 Leaf p(x2) @@ -726,6 +738,7 @@ conditional 2: Hybrid P( x2 | m0 m1) d = [ -10.1489 ] mean: 1 elements x2: -1.0099 + logNormalizationConstant: 1.38857 No noise model 0 1 Leaf p(x2) @@ -733,6 +746,7 @@ conditional 2: Hybrid P( x2 | m0 m1) d = [ -10.1479 ] mean: 1 elements x2: -1.0098 + logNormalizationConstant: 1.38857 No noise model 1 Choice(m0) @@ -741,6 +755,7 @@ conditional 2: Hybrid P( x2 | m0 m1) d = [ -10.0504 ] mean: 1 elements x2: -1.0001 + logNormalizationConstant: 1.38857 No noise model 1 1 Leaf p(x2) @@ -748,6 +763,7 @@ conditional 2: Hybrid P( x2 | m0 m1) d = [ -10.0494 ] mean: 1 elements x2: -1 + logNormalizationConstant: 1.38857 No noise model )"; diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 4ec1d8b95..fb7058282 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -121,6 +121,7 @@ namespace gtsam { const auto mean = solve({}); // solve for mean. mean.print(" mean", formatter); } + cout << " logNormalizationConstant: " << logNormalizationConstant() << std::endl; if (model_) model_->print(" Noise model: "); else diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index a4a722012..dcd821889 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -516,6 +516,7 @@ TEST(GaussianConditional, Print) { " d = [ 20 40 ]\n" " mean: 1 elements\n" " x0: 20 40\n" + " logNormalizationConstant: -4.0351\n" "isotropic dim=2 sigma=3\n"; EXPECT(assert_print_equal(expected, conditional, "GaussianConditional")); @@ -530,6 +531,7 @@ TEST(GaussianConditional, Print) { " S[x1] = [ -1 -2 ]\n" " [ -3 -4 ]\n" " d = [ 20 40 ]\n" + " logNormalizationConstant: -4.0351\n" "isotropic dim=2 sigma=3\n"; EXPECT(assert_print_equal(expected1, conditional1, "GaussianConditional")); @@ -545,6 +547,7 @@ TEST(GaussianConditional, Print) { " S[y1] = [ -5 -6 ]\n" " [ -7 -8 ]\n" " d = [ 20 40 ]\n" + " logNormalizationConstant: -4.0351\n" "isotropic dim=2 sigma=3\n"; EXPECT(assert_print_equal(expected2, conditional2, "GaussianConditional")); } From 114c86fc6bd1f81c57d5c9653cf15f8dd39596c8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 3 Jan 2024 16:15:37 -0500 Subject: [PATCH 25/71] GaussianConditional wrapper for arbitrary number of keys --- gtsam/linear/linear.i | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index d4a045f09..6862646ae 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -489,12 +489,17 @@ virtual class GaussianConditional : gtsam::JacobianFactor { GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, size_t name2, Matrix T, const gtsam::noiseModel::Diagonal* sigmas); + GaussianConditional(const vector> terms, + size_t nrFrontals, Vector d, + const gtsam::noiseModel::Diagonal* sigmas); // Constructors with no noise model GaussianConditional(size_t key, Vector d, Matrix R); GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S); GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, size_t name2, Matrix T); + GaussianConditional(const gtsam::KeyVector& keys, size_t nrFrontals, + const gtsam::VerticalBlockMatrix& augmentedMatrix); // Named constructors static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key, From 82e0c0dae10e58e5a15d3930baeb031585e7946f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 3 Jan 2024 16:26:05 -0500 Subject: [PATCH 26/71] take comment all the way --- gtsam/linear/GaussianConditional.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 4ec1d8b95..d59070aaf 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -186,7 +186,10 @@ namespace gtsam { size_t n = d().size(); // Sigma = (R'R)^{-1}, det(Sigma) = det((R'R)^{-1}) = det(R'R)^{-1} // log det(Sigma) = -log(det(R'R)) = -2*log(det(R)) - // Hence, log det(Sigma)) = - 2.0 * logDeterminant() + // Hence, log det(Sigma)) = -2.0 * logDeterminant() + // which gives log = -0.5*n*log(2*pi) - 0.5*(-2.0 * logDeterminant()) + // = -0.5*n*log(2*pi) + (0.5*2.0 * logDeterminant()) + // = -0.5*n*log(2*pi) + logDeterminant() return -0.5 * n * log2pi + logDeterminant(); } From 8a61c49bb3ca447af3d7bac86c1694a73c0b398e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 3 Jan 2024 16:32:21 -0500 Subject: [PATCH 27/71] add model_selection method to HybridBayesNet --- gtsam/hybrid/HybridBayesNet.cpp | 50 ++++++++++++++++----------------- gtsam/hybrid/HybridBayesNet.h | 13 +++++++++ 2 files changed, 37 insertions(+), 26 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 0352d7962..81f4badea 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -283,35 +283,20 @@ GaussianBayesNetValTree HybridBayesNet::assembleTree() const { } /* ************************************************************************* */ -HybridValues HybridBayesNet::optimize() const { - // Collect all the discrete factors to compute MPE - DiscreteFactorGraph discrete_fg; - +AlgebraicDecisionTree HybridBayesNet::model_selection() const { /* - Perform the integration of L(X;M,Z)P(X|M) - which is the model selection term. + To perform model selection, we need: + q(mu; M, Z) * sqrt((2*pi)^n*det(Sigma)) - By Bayes' rule, P(X|M,Z) ∝ L(X;M,Z)P(X|M), - hence L(X;M,Z)P(X|M) is the unnormalized probabilty of - the joint Gaussian distribution. + If q(mu; M, Z) = exp(-error) & k = 1.0 / sqrt((2*pi)^n*det(Sigma)) + thus, q * sqrt((2*pi)^n*det(Sigma)) = q/k = exp(log(q/k)) + = exp(log(q) - log(k)) = exp(-error - log(k)) + = exp(-(error + log(k))), + where error is computed at the corresponding MAP point, gbn.error(mu). - This can be computed by multiplying all the exponentiated errors - of each of the conditionals, which we do below in hybrid case. - */ - /* - To perform model selection, we need: - q(mu; M, Z) * sqrt((2*pi)^n*det(Sigma)) + So we compute (error + log(k)) and exponentiate later + */ - If q(mu; M, Z) = exp(-error) & k = 1.0 / sqrt((2*pi)^n*det(Sigma)) - thus, q * sqrt((2*pi)^n*det(Sigma)) = q/k = exp(log(q/k)) - = exp(log(q) - log(k)) = exp(-error - log(k)) - = exp(-(error + log(k))), - where error is computed at the corresponding MAP point, gbn.error(mu). - - So we compute (error + log(k)) and exponentiate later - */ - - std::set discreteKeySet; GaussianBayesNetValTree bnTree = assembleTree(); GaussianBayesNetValTree bn_error = bnTree.apply( @@ -356,6 +341,19 @@ HybridValues HybridBayesNet::optimize() const { [&max_log](const double &x) { return std::exp(x - max_log); }); model_selection = model_selection.normalize(model_selection.sum()); + return model_selection; +} + +/* ************************************************************************* */ +HybridValues HybridBayesNet::optimize() const { + // Collect all the discrete factors to compute MPE + DiscreteFactorGraph discrete_fg; + + // Compute model selection term + AlgebraicDecisionTree model_selection_term = model_selection(); + + // Get the set of all discrete keys involved in model selection + std::set discreteKeySet; for (auto &&conditional : *this) { if (conditional->isDiscrete()) { discrete_fg.push_back(conditional->asDiscrete()); @@ -380,7 +378,7 @@ HybridValues HybridBayesNet::optimize() const { if (discreteKeySet.size() > 0) { discrete_fg.push_back(DecisionTreeFactor( DiscreteKeys(discreteKeySet.begin(), discreteKeySet.end()), - model_selection)); + model_selection_term)); } // Solve for the MPE diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index f0cdbaaf9..8acdd5b1b 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -120,6 +120,19 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { GaussianBayesNetValTree assembleTree() const; + /* + Perform the integration of L(X;M,Z)P(X|M) + which is the model selection term. + + By Bayes' rule, P(X|M,Z) ∝ L(X;M,Z)P(X|M), + hence L(X;M,Z)P(X|M) is the unnormalized probabilty of + the joint Gaussian distribution. + + This can be computed by multiplying all the exponentiated errors + of each of the conditionals. + */ + AlgebraicDecisionTree model_selection() const; + /** * @brief Solve the HybridBayesNet by first computing the MPE of all the * discrete variables and then optimizing the continuous variables based on From 3ba54eb25da317a81327eae0de3a4a382d9bee2d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 3 Jan 2024 16:37:49 -0500 Subject: [PATCH 28/71] improved docstrings --- gtsam/hybrid/GaussianMixture.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index a52cd6c82..65c631ece 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -67,13 +67,14 @@ class GTSAM_EXPORT GaussianMixture double logConstant_; ///< log of the normalization constant. /** - * @brief Convert a DecisionTree of factors into a DT of Gaussian FGs. + * @brief Convert a DecisionTree of factors into + * a DecisionTree of Gaussian factor graphs. */ GaussianFactorGraphTree asGaussianFactorGraphTree() const; /** * @brief Convert a DecisionTree of conditionals into - * a DT of Gaussian Bayes nets. + * a DecisionTree of Gaussian Bayes nets. */ GaussianBayesNetTree asGaussianBayesNetTree() const; From bb95cd40d6a4434bf3bacffd155252a24e9266ef Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 3 Jan 2024 16:38:25 -0500 Subject: [PATCH 29/71] remove `using std::dynamic_pointer_cast;` --- gtsam/hybrid/HybridBayesNet.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 81f4badea..933ef2703 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -26,8 +26,6 @@ static std::mt19937_64 kRandomNumberGenerator(42); namespace gtsam { -using std::dynamic_pointer_cast; - /* ************************************************************************ */ // Throw a runtime exception for method specified in string s, // and conditional f: @@ -253,9 +251,9 @@ GaussianBayesNetValTree HybridBayesNet::assembleTree() const { for (auto &f : factors_) { // TODO(dellaert): just use a virtual method defined in HybridFactor. - if (auto gm = dynamic_pointer_cast(f)) { + if (auto gm = std::dynamic_pointer_cast(f)) { result = gm->add(result); - } else if (auto hc = dynamic_pointer_cast(f)) { + } else if (auto hc = std::dynamic_pointer_cast(f)) { if (auto gm = hc->asMixture()) { result = gm->add(result); } else if (auto g = hc->asGaussian()) { @@ -265,7 +263,7 @@ GaussianBayesNetValTree HybridBayesNet::assembleTree() const { // TODO(dellaert): in C++20, we can use std::visit. continue; } - } else if (dynamic_pointer_cast(f)) { + } else if (std::dynamic_pointer_cast(f)) { // Don't do anything for discrete-only factors // since we want to evaluate continuous values only. continue; From 6d50de8c1cc10f7b778373666c46dd1e730d1f5e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 3 Jan 2024 16:40:04 -0500 Subject: [PATCH 30/71] docstring for HybridBayesNet::assembleTree --- gtsam/hybrid/HybridBayesNet.h | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 8acdd5b1b..cedb379c2 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -118,6 +118,14 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { return evaluate(values); } + /** + * @brief Assemble a DecisionTree of (GaussianBayesNet, double) leaves for + * each discrete assignment. + * The included double value is used to make + * constructing the model selection term cleaner and more efficient. + * + * @return GaussianBayesNetValTree + */ GaussianBayesNetValTree assembleTree() const; /* From 0430fee3777482886ec3ca510d79bbc073186f5a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 7 Jan 2024 15:49:33 -0500 Subject: [PATCH 31/71] improved naming and documentation --- gtsam/hybrid/HybridBayesNet.cpp | 18 +++++++++--------- gtsam/hybrid/HybridBayesNet.h | 9 ++++++--- 2 files changed, 15 insertions(+), 12 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index e9870e6bf..23ee72215 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -281,7 +281,7 @@ GaussianBayesNetValTree HybridBayesNet::assembleTree() const { } /* ************************************************************************* */ -AlgebraicDecisionTree HybridBayesNet::model_selection() const { +AlgebraicDecisionTree HybridBayesNet::modelSelection() const { /* To perform model selection, we need: q(mu; M, Z) * sqrt((2*pi)^n*det(Sigma)) @@ -330,16 +330,16 @@ AlgebraicDecisionTree HybridBayesNet::model_selection() const { }); // Compute model selection term (with help from ADT methods) - AlgebraicDecisionTree model_selection_term = + AlgebraicDecisionTree modelSelectionTerm = (errorTree + log_norm_constants) * -1; - double max_log = model_selection_term.max(); - AlgebraicDecisionTree model_selection = DecisionTree( - model_selection_term, + double max_log = modelSelectionTerm.max(); + modelSelectionTerm = DecisionTree( + modelSelectionTerm, [&max_log](const double &x) { return std::exp(x - max_log); }); - model_selection = model_selection.normalize(model_selection.sum()); + modelSelectionTerm = modelSelectionTerm.normalize(modelSelectionTerm.sum()); - return model_selection; + return modelSelectionTerm; } /* ************************************************************************* */ @@ -348,7 +348,7 @@ HybridValues HybridBayesNet::optimize() const { DiscreteFactorGraph discrete_fg; // Compute model selection term - AlgebraicDecisionTree model_selection_term = model_selection(); + AlgebraicDecisionTree modelSelectionTerm = modelSelection(); // Get the set of all discrete keys involved in model selection std::set discreteKeySet; @@ -376,7 +376,7 @@ HybridValues HybridBayesNet::optimize() const { if (discreteKeySet.size() > 0) { discrete_fg.push_back(DecisionTreeFactor( DiscreteKeys(discreteKeySet.begin(), discreteKeySet.end()), - model_selection_term)); + modelSelectionTerm)); } // Solve for the MPE diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 757e60aea..9d16a4e14 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -129,8 +129,11 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { GaussianBayesNetValTree assembleTree() const; /* - Perform the integration of L(X;M,Z)P(X|M) - which is the model selection term. + Compute L(M;Z), the likelihood of the discrete model M + given the measurements Z. + This is called the model selection term. + + To do so, we perform the integration of L(M;Z) ∝ L(X;M,Z)P(X|M). By Bayes' rule, P(X|M,Z) ∝ L(X;M,Z)P(X|M), hence L(X;M,Z)P(X|M) is the unnormalized probabilty of @@ -139,7 +142,7 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { This can be computed by multiplying all the exponentiated errors of each of the conditionals. */ - AlgebraicDecisionTree model_selection() const; + AlgebraicDecisionTree modelSelection() const; /** * @brief Solve the HybridBayesNet by first computing the MPE of all the From afcb93390ad05a93a8d9966e16184b935d76f4a8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 7 Jan 2024 15:54:40 -0500 Subject: [PATCH 32/71] document return type --- gtsam/hybrid/HybridBayesNet.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 9d16a4e14..fe9010b1f 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -141,6 +141,8 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { This can be computed by multiplying all the exponentiated errors of each of the conditionals. + + Return a tree where each leaf value is L(M_i;Z). */ AlgebraicDecisionTree modelSelection() const; From 5cdbacfd4408db583c1726dd301b64fe60a659ca Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 7 Jan 2024 20:23:58 -0500 Subject: [PATCH 33/71] test and fix for issue 301 --- gtsam/nonlinear/ISAM2.cpp | 15 +++-- tests/testDoglegOptimizer.cpp | 122 ++++++++++++++++++++++++++++++++-- 2 files changed, 126 insertions(+), 11 deletions(-) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 0b5449b58..47857a2a2 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -424,6 +424,11 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, ISAM2Result result(params_.enableDetailedResults); UpdateImpl update(params_, updateParams); + // Initialize any new variables \Theta_{new} and add + // \Theta:=\Theta\cup\Theta_{new}. + // Needed before delta update if using Dogleg optimizer. + addVariables(newTheta, result.details()); + // Update delta if we need it to check relinearization later if (update.relinarizationNeeded(update_count_)) updateDelta(updateParams.forceFullSolve); @@ -435,9 +440,7 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, update.computeUnusedKeys(newFactors, variableIndex_, result.keysWithRemovedFactors, &result.unusedKeys); - // 2. Initialize any new variables \Theta_{new} and add - // \Theta:=\Theta\cup\Theta_{new}. - addVariables(newTheta, result.details()); + // 2. Compute new error to check for relinearization if (params_.evaluateNonlinearError) update.error(nonlinearFactors_, calculateEstimate(), &result.errorBefore); @@ -731,6 +734,7 @@ void ISAM2::updateDelta(bool forceFullSolve) const { effectiveWildfireThreshold, &delta_); deltaReplacedMask_.clear(); gttoc(Wildfire_update); + } else if (std::holds_alternative(params_.optimizationParams)) { // If using Dogleg, do a Dogleg step const ISAM2DoglegParams& doglegParams = @@ -769,9 +773,8 @@ void ISAM2::updateDelta(bool forceFullSolve) const { gttic(Copy_dx_d); // Update Delta and linear step doglegDelta_ = doglegResult.delta; - delta_ = - doglegResult - .dx_d; // Copy the VectorValues containing with the linear solution + // Copy the VectorValues containing with the linear solution + delta_ = doglegResult.dx_d; gttoc(Copy_dx_d); } else { throw std::runtime_error("iSAM2: unknown ISAM2Params type"); diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index e64e5ab7a..d7de63d3d 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -24,10 +24,9 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include "examples/SFMdata.h" #include @@ -36,7 +35,6 @@ using namespace gtsam; // Convenience for named keys using symbol_shorthand::X; -using symbol_shorthand::L; /* ************************************************************************* */ TEST(DoglegOptimizer, ComputeBlend) { @@ -185,6 +183,120 @@ TEST(DoglegOptimizer, Constraint) { #endif } +/* ************************************************************************* */ +TEST(DogLegOptimizer, VariableUpdate) { + // Make the typename short so it looks much cleaner + typedef SmartProjectionPoseFactor SmartFactor; + + // create a typedef to the camera type + typedef PinholePose Camera; + // Define the camera calibration parameters + Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + + // Define the camera observation noise model + noiseModel::Isotropic::shared_ptr measurementNoise = + noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + + // Create the set of ground-truth landmarks and poses + vector points = createPoints(); + vector poses = createPoses(); + + // Create a factor graph + NonlinearFactorGraph graph; + + ISAM2DoglegParams doglegparams = ISAM2DoglegParams(); + doglegparams.verbose = false; + ISAM2Params isam2_params; + isam2_params.evaluateNonlinearError = true; + isam2_params.relinearizeThreshold = 0.0; + isam2_params.enableRelinearization = true; + isam2_params.optimizationParams = doglegparams; + isam2_params.relinearizeSkip = 1; + ISAM2 isam2(isam2_params); + + // Simulated measurements from each camera pose, adding them to the factor + // graph + unordered_map smart_factors; + for (size_t j = 0; j < points.size(); ++j) { + // every landmark represent a single landmark, we use shared pointer to init + // the factor, and then insert measurements. + SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K)); + + for (size_t i = 0; i < poses.size(); ++i) { + // generate the 2D measurement + Camera camera(poses[i], K); + Point2 measurement = camera.project(points[j]); + + // call add() function to add measurement into a single factor, here we + // need to add: + // 1. the 2D measurement + // 2. the corresponding camera's key + // 3. camera noise model + // 4. camera calibration + + // add only first 3 measurements and update the later measurements + // incrementally + if (i < 3) smartfactor->add(measurement, i); + } + + // insert the smart factor in the graph + smart_factors[j] = smartfactor; + graph.push_back(smartfactor); + } + + // Add a prior on pose x0. This indirectly specifies where the origin is. + // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); + graph.emplace_shared >(0, poses[0], noise); + + // Because the structure-from-motion problem has a scale ambiguity, the + // problem is still under-constrained. Here we add a prior on the second pose + // x1, so this will fix the scale by indicating the distance between x0 and + // x1. Because these two are fixed, the rest of the poses will be also be + // fixed. + graph.emplace_shared >(1, poses[1], + noise); // add directly to graph + + // Create the initial estimate to the solution + // Intentionally initialize the variables off from the ground truth + Values initialEstimate; + Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); + for (size_t i = 0; i < 3; ++i) + initialEstimate.insert(i, poses[i].compose(delta)); + // initialEstimate.print("Initial Estimates:\n"); + + // Optimize the graph and print results + isam2.update(graph, initialEstimate); + Values result = isam2.calculateEstimate(); + // result.print("Results:\n"); + + // we add new measurements from this pose + size_t pose_idx = 3; + + // Now update existing smart factors with new observations + for (size_t j = 0; j < points.size(); ++j) { + SmartFactor::shared_ptr smartfactor = smart_factors[j]; + + // add the 4th measurement + Camera camera(poses[pose_idx], K); + Point2 measurement = camera.project(points[j]); + smartfactor->add(measurement, pose_idx); + } + + graph.resize(0); + initialEstimate.clear(); + + // update initial estimate for the new pose + initialEstimate.insert(pose_idx, poses[pose_idx].compose(delta)); + + // this should break the system + isam2.update(graph, initialEstimate); + result = isam2.calculateEstimate(); + EXPECT(std::find(result.keys().begin(), result.keys().end(), pose_idx) != + result.keys().end()); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From 4c8e4973bf00d6d01379ab0b01bc481a653ea86a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 8 Jan 2024 08:52:47 -0500 Subject: [PATCH 34/71] update Rot2::fromCosSin docstring --- gtsam/geometry/Rot2.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 63b914434..6bb97ff6c 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -68,7 +68,7 @@ namespace gtsam { return fromAngle(theta * degree); } - /// Named constructor from cos(theta),sin(theta) pair, will *not* normalize! + /// Named constructor from cos(theta),sin(theta) pair static Rot2 fromCosSin(double c, double s); /** From c5bfd524e0ca8fa8c49fe5ad8183492b748b81ad Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 12 Jan 2024 10:28:02 -0500 Subject: [PATCH 35/71] better printing of GaussianMixtureFactor --- gtsam/hybrid/GaussianMixtureFactor.cpp | 4 +++- gtsam/hybrid/GaussianMixtureFactor.h | 5 ++--- gtsam/hybrid/tests/testGaussianMixtureFactor.cpp | 3 ++- gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp | 2 ++ 4 files changed, 9 insertions(+), 5 deletions(-) diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index a3db16d04..4e138acfa 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -54,7 +54,9 @@ bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const { /* *******************************************************************************/ void GaussianMixtureFactor::print(const std::string &s, const KeyFormatter &formatter) const { - HybridFactor::print(s, formatter); + std::cout << (s.empty() ? "" : s + "\n"); + std::cout << "GaussianMixtureFactor" << std::endl; + HybridFactor::print("", formatter); std::cout << "{\n"; if (factors_.empty()) { std::cout << " empty" << std::endl; diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index 63ca9e923..913426a98 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -107,9 +107,8 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { bool equals(const HybridFactor &lf, double tol = 1e-9) const override; - void print( - const std::string &s = "GaussianMixtureFactor\n", - const KeyFormatter &formatter = DefaultKeyFormatter) const override; + void print(const std::string &s = "", const KeyFormatter &formatter = + DefaultKeyFormatter) const override; /// @} /// @name Standard API diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 9cc7e6bfd..11b01f074 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -106,7 +106,8 @@ TEST(GaussianMixtureFactor, Printing) { GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors); std::string expected = - R"(Hybrid [x1 x2; 1]{ + R"(GaussianMixtureFactor +Hybrid [x1 x2; 1]{ Choice(1) 0 Leaf : A[x1] = [ diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 93081d309..0d4bf27c4 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -510,6 +510,7 @@ factor 0: b = [ -10 ] No noise model factor 1: +GaussianMixtureFactor Hybrid [x0 x1; m0]{ Choice(m0) 0 Leaf : @@ -534,6 +535,7 @@ Hybrid [x0 x1; m0]{ } factor 2: +GaussianMixtureFactor Hybrid [x1 x2; m1]{ Choice(m1) 0 Leaf : From a9cf4a0779b42ed1f5bfe208c474acfae495bcc5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 2 Jul 2024 20:18:24 -0400 Subject: [PATCH 36/71] fix namespacing --- gtsam/linear/linear.i | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 577cc8acf..3a629f349 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -510,8 +510,8 @@ virtual class GaussianConditional : gtsam::JacobianFactor { GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R, size_t name1, gtsam::Matrix S, size_t name2, gtsam::Matrix T, const gtsam::noiseModel::Diagonal* sigmas); - GaussianConditional(const vector> terms, - size_t nrFrontals, Vector d, + GaussianConditional(const vector> terms, + size_t nrFrontals, gtsam::Vector d, const gtsam::noiseModel::Diagonal* sigmas); // Constructors with no noise model From 113a7f8e6b0eeae0d9dbf4170093e703fce12427 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 5 Aug 2024 11:58:51 -0400 Subject: [PATCH 37/71] added more comments and compute GaussianMixture before tau --- gtsam/hybrid/GaussianMixtureFactor.h | 3 ++- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 17 ++++++++++------- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index 913426a98..b1be6ef1d 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -134,7 +134,8 @@ class GTSAM_EXPORT GaussianMixtureFactor : 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; + AlgebraicDecisionTree errorTree( + const VectorValues &continuousValues) const; /** * @brief Compute the log-likelihood, including the log-normalizing constant. diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 42402076e..f1b79b123 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -296,7 +296,10 @@ static std::shared_ptr createDiscreteFactor( // Logspace version of: // exp(-factor->error(kEmpty)) / conditional->normalizationConstant(); - return -factor->error(kEmpty) - conditional->logNormalizationConstant(); + // We take negative of the logNormalizationConstant `log(1/k)` + // to get `log(k)`. + // factor->print("Discrete Separator"); + return -factor->error(kEmpty) + (-conditional->logNormalizationConstant()); }; AlgebraicDecisionTree logProbabilities( @@ -368,6 +371,12 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // Perform elimination! DecisionTree eliminationResults(factorGraphTree, eliminate); + // Create the GaussianMixture from the conditionals + GaussianMixture::Conditionals conditionals( + eliminationResults, [](const Result &pair) { return pair.first; }); + auto gaussianMixture = std::make_shared( + frontalKeys, continuousSeparator, discreteSeparator, conditionals); + // If there are no more continuous parents we create a DiscreteFactor with the // error for each discrete choice. Otherwise, create a GaussianMixtureFactor // on the separator, taking care to correct for conditional constants. @@ -377,12 +386,6 @@ hybridElimination(const HybridGaussianFactorGraph &factors, : createGaussianMixtureFactor(eliminationResults, continuousSeparator, discreteSeparator); - // Create the GaussianMixture from the conditionals - GaussianMixture::Conditionals conditionals( - eliminationResults, [](const Result &pair) { return pair.first; }); - auto gaussianMixture = std::make_shared( - frontalKeys, continuousSeparator, discreteSeparator, conditionals); - return {std::make_shared(gaussianMixture), newFactor}; } From 2430abb4bc71ed1bf4bec4079d62c4bc115c2c5b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 7 Aug 2024 09:22:20 -0400 Subject: [PATCH 38/71] test for different error values in BN from MixtureFactor --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 1 - gtsam/hybrid/tests/testMixtureFactor.cpp | 53 +++++++++++++++++++++- 2 files changed, 51 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index f1b79b123..f05dfd423 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -298,7 +298,6 @@ static std::shared_ptr createDiscreteFactor( // exp(-factor->error(kEmpty)) / conditional->normalizationConstant(); // We take negative of the logNormalizationConstant `log(1/k)` // to get `log(k)`. - // factor->print("Discrete Separator"); return -factor->error(kEmpty) + (-conditional->logNormalizationConstant()); }; diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp index 006a3b026..c03df4d61 100644 --- a/gtsam/hybrid/tests/testMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testMixtureFactor.cpp @@ -161,6 +161,35 @@ TEST(MixtureFactor, DifferentMeans) { DiscreteValues{{M(1), 1}, {M(2), 0}}); EXPECT(assert_equal(expected, actual)); + + { + DiscreteValues dv{{M(1), 0}, {M(2), 0}}; + VectorValues cont = bn->optimize(dv); + double error = bn->error(HybridValues(cont, dv)); + // regression + EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9); + } + { + DiscreteValues dv{{M(1), 0}, {M(2), 1}}; + VectorValues cont = bn->optimize(dv); + double error = bn->error(HybridValues(cont, dv)); + // regression + EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9); + } + { + DiscreteValues dv{{M(1), 1}, {M(2), 0}}; + VectorValues cont = bn->optimize(dv); + double error = bn->error(HybridValues(cont, dv)); + // regression + EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9); + } + { + DiscreteValues dv{{M(1), 1}, {M(2), 1}}; + VectorValues cont = bn->optimize(dv); + double error = bn->error(HybridValues(cont, dv)); + // regression + EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9); + } } /* ************************************************************************* */ @@ -217,15 +246,35 @@ TEST(MixtureFactor, DifferentCovariances) { auto hbn = mixture_fg.eliminateSequential(); - HybridValues actual_values = hbn->optimize(); - VectorValues cv; cv.insert(X(1), Vector1(0.0)); cv.insert(X(2), Vector1(0.0)); + + // Check that we get different error values at the MLE point μ. + AlgebraicDecisionTree errorTree = hbn->errorTree(cv); + auto cond0 = hbn->at(0)->asMixture(); + auto cond1 = hbn->at(1)->asMixture(); + auto discrete_cond = hbn->at(2)->asDiscrete(); + + HybridValues hv0(cv, DiscreteValues{{M(1), 0}}); + HybridValues hv1(cv, DiscreteValues{{M(1), 1}}); + AlgebraicDecisionTree expectedErrorTree( + m1, + cond0->error(hv0) // cond0(0)->logNormalizationConstant() + // - cond0(1)->logNormalizationConstant + + cond1->error(hv0) + discrete_cond->error(DiscreteValues{{M(1), 0}}), + cond0->error(hv1) // cond1(0)->logNormalizationConstant() + // - cond1(1)->logNormalizationConstant + + cond1->error(hv1) + + discrete_cond->error(DiscreteValues{{M(1), 0}})); + EXPECT(assert_equal(expectedErrorTree, errorTree)); + DiscreteValues dv; dv.insert({M(1), 1}); HybridValues expected_values(cv, dv); + HybridValues actual_values = hbn->optimize(); + EXPECT(assert_equal(expected_values, actual_values)); } From 6dfd5671b1303785f28717388f76e3578fee45df Mon Sep 17 00:00:00 2001 From: Gary Date: Wed, 7 Aug 2024 15:46:13 -0400 Subject: [PATCH 39/71] Adding test and fix for issue #1596 A Non-active constraint returns a `nullptr`, which needs to be checked for when gathering the keys in `EliminateSymbolic`. --- gtsam/symbolic/SymbolicFactor-inst.h | 4 ++- tests/testGaussianISAM2.cpp | 50 ++++++++++++++++++++++++++++ 2 files changed, 53 insertions(+), 1 deletion(-) diff --git a/gtsam/symbolic/SymbolicFactor-inst.h b/gtsam/symbolic/SymbolicFactor-inst.h index db0eb05ca..a5410aad5 100644 --- a/gtsam/symbolic/SymbolicFactor-inst.h +++ b/gtsam/symbolic/SymbolicFactor-inst.h @@ -42,7 +42,9 @@ namespace gtsam // Gather all keys KeySet allKeys; for(const std::shared_ptr& factor: factors) { - allKeys.insert(factor->begin(), factor->end()); + // Non-active factors are nullptr + if (factor) + allKeys.insert(factor->begin(), factor->end()); } // Check keys diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 1ea60dac9..721ed51c0 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -994,6 +994,56 @@ TEST(ISAM2, calculate_nnz) EXPECT_LONGS_EQUAL(expected, actual); } +class FixActiveFactor : public NoiseModelFactorN { + using Base = NoiseModelFactorN; + bool is_active_; + +public: + FixActiveFactor(const gtsam::Key& key, const bool active) + : Base(nullptr, key), is_active_(active) {} + + virtual bool active(const gtsam::Values &values) const override { + return is_active_; + } + + virtual Vector + evaluateError(const Vector2& x, + Base::OptionalMatrixTypeT H = nullptr) const override { + if (H) { + *H = Vector2::Identity(); + } + return Vector2::Zero(); + } +}; + +TEST(ActiveFactorTesting, Issue1596) { + // Issue1596: When a derived Nonlinear Factor is not active, the linearization returns a nullptr (NonlinearFactor.cpp:156), which + // causes an error when `EliminateSymbolic` is called (SymbolicFactor-inst.h:45) due to not checking if the factor is nullptr. + const gtsam::Key key{Symbol('x', 0)}; + + ISAM2 isam; + Values values; + NonlinearFactorGraph graph; + + // Insert an active factor + values.insert(key, Vector2::Zero()); + graph.emplace_shared(key, true); + + // No problem here + isam.update(graph, values); + + graph = NonlinearFactorGraph(); + + // Inserting a factor that is never active + graph.emplace_shared(key, false); + + // This call throws the error if the pointer is not validated on (SymbolicFactor-inst.h:45) + isam.update(graph); + + // If the bug is fixed, this line is reached. + EXPECT(isam.getFactorsUnsafe().size() == 2); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ From 0f23eedbd063fe982f913392267b02b3e3617d30 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 19 Aug 2024 11:39:02 -0400 Subject: [PATCH 40/71] fix issue in geometry.i file --- gtsam/geometry/geometry.i | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 3d816fc25..095a350dd 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -856,7 +856,7 @@ class Cal3_S2Stereo { gtsam::Matrix K() const; gtsam::Point2 principalPoint() const; double baseline() const; - Vector6 vector() const; + gtsam::Vector6 vector() const; gtsam::Matrix inverse() const; }; From 3c722acedc220b4daf145104f494c3d3f9d6e172 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 20 Aug 2024 07:51:00 -0400 Subject: [PATCH 41/71] update GaussianMixtureFactor to record normalizers, and add unit tests --- gtsam/hybrid/GaussianMixtureFactor.cpp | 79 ++++++++- gtsam/hybrid/GaussianMixtureFactor.h | 13 +- .../tests/testGaussianMixtureFactor.cpp | 167 +++++++++++++++++- 3 files changed, 250 insertions(+), 9 deletions(-) diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index 4e138acfa..e519cefe6 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -28,11 +28,86 @@ namespace gtsam { +/** + * @brief Helper function to correct the [A|b] matrices in the factor components + * with the normalizer values. + * This is done by storing the normalizer value in + * the `b` vector as an additional row. + * + * @param factors DecisionTree of GaussianFactor shared pointers. + * @param varyingNormalizers Flag indicating the normalizers are different for + * each component. + * @return GaussianMixtureFactor::Factors + */ +GaussianMixtureFactor::Factors correct( + const GaussianMixtureFactor::Factors &factors, bool varyingNormalizers) { + if (!varyingNormalizers) { + return factors; + } + + // First compute all the sqrt(|2 pi Sigma|) terms + auto computeNormalizers = [](const GaussianMixtureFactor::sharedFactor &gf) { + auto jf = std::dynamic_pointer_cast(gf); + // If we have, say, a Hessian factor, then no need to do anything + if (!jf) return 0.0; + + auto model = jf->get_model(); + // If there is no noise model, there is nothing to do. + if (!model) { + return 0.0; + } + // Since noise models are Gaussian, we can get the logDeterminant using the + // same trick as in GaussianConditional + double logDetR = + model->R().diagonal().unaryExpr([](double x) { return log(x); }).sum(); + double logDeterminantSigma = -2.0 * logDetR; + + size_t n = model->dim(); + constexpr double log2pi = 1.8378770664093454835606594728112; + return n * log2pi + logDeterminantSigma; + }; + + AlgebraicDecisionTree log_normalizers = + DecisionTree(factors, computeNormalizers); + + // Find the minimum value so we can "proselytize" to positive values. + // Done because we can't have sqrt of negative numbers. + double min_log_normalizer = log_normalizers.min(); + log_normalizers = log_normalizers.apply( + [&min_log_normalizer](double n) { return n - min_log_normalizer; }); + + // Finally, update the [A|b] matrices. + auto update = [&log_normalizers]( + const Assignment &assignment, + const GaussianMixtureFactor::sharedFactor &gf) { + auto jf = std::dynamic_pointer_cast(gf); + if (!jf) return gf; + // If there is no noise model, there is nothing to do. + if (!jf->get_model()) return gf; + // If the log_normalizer is 0, do nothing + if (log_normalizers(assignment) == 0.0) return gf; + + GaussianFactorGraph gfg; + gfg.push_back(jf); + + Vector c(1); + c << std::sqrt(log_normalizers(assignment)); + auto constantFactor = std::make_shared(c); + + gfg.push_back(constantFactor); + return std::dynamic_pointer_cast( + std::make_shared(gfg)); + }; + return factors.apply(update); +} + /* *******************************************************************************/ GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, - const Factors &factors) - : Base(continuousKeys, discreteKeys), factors_(factors) {} + const Factors &factors, + bool varyingNormalizers) + : Base(continuousKeys, discreteKeys), + factors_(correct(factors, varyingNormalizers)) {} /* *******************************************************************************/ bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const { diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index b1be6ef1d..588501bbe 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -82,10 +82,13 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { * their cardinalities. * @param factors The decision tree of Gaussian factors stored as the mixture * density. + * @param varyingNormalizers Flag indicating factor components have varying + * normalizer values. */ GaussianMixtureFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, - const Factors &factors); + const Factors &factors, + bool varyingNormalizers = false); /** * @brief Construct a new GaussianMixtureFactor object using a vector of @@ -94,12 +97,16 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { * @param continuousKeys Vector of keys for continuous factors. * @param discreteKeys Vector of discrete keys. * @param factors Vector of gaussian factor shared pointers. + * @param varyingNormalizers Flag indicating factor components have varying + * normalizer values. */ GaussianMixtureFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, - const std::vector &factors) + const std::vector &factors, + bool varyingNormalizers = false) : GaussianMixtureFactor(continuousKeys, discreteKeys, - Factors(discreteKeys, factors)) {} + Factors(discreteKeys, factors), + varyingNormalizers) {} /// @} /// @name Testable diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 11b01f074..55ecce939 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -22,9 +22,13 @@ #include #include #include +#include +#include #include #include #include +#include +#include // Include for test suite #include @@ -56,7 +60,6 @@ TEST(GaussianMixtureFactor, Sum) { auto b = Matrix::Zero(2, 1); Vector2 sigmas; sigmas << 1, 2; - auto model = noiseModel::Diagonal::Sigmas(sigmas, true); auto f10 = std::make_shared(X(1), A1, X(2), A2, b); auto f11 = std::make_shared(X(1), A1, X(2), A2, b); @@ -179,7 +182,8 @@ TEST(GaussianMixtureFactor, Error) { continuousValues.insert(X(2), Vector2(1, 1)); // error should return a tree of errors, with nodes for each discrete value. - AlgebraicDecisionTree error_tree = mixtureFactor.errorTree(continuousValues); + AlgebraicDecisionTree error_tree = + mixtureFactor.errorTree(continuousValues); std::vector discrete_keys = {m1}; // Error values for regression test @@ -192,8 +196,163 @@ TEST(GaussianMixtureFactor, Error) { DiscreteValues discreteValues; discreteValues[m1.first] = 1; EXPECT_DOUBLES_EQUAL( - 4.0, mixtureFactor.error({continuousValues, discreteValues}), - 1e-9); + 4.0, mixtureFactor.error({continuousValues, discreteValues}), 1e-9); +} + +/* ************************************************************************* */ +// Test components with differing means +TEST(GaussianMixtureFactor, DifferentMeans) { + DiscreteKey m1(M(1), 2), m2(M(2), 2); + + Values values; + double x1 = 0.0, x2 = 1.75, x3 = 2.60; + values.insert(X(1), x1); + values.insert(X(2), x2); + values.insert(X(3), x3); + + auto model0 = noiseModel::Isotropic::Sigma(1, 1e-0); + auto model1 = noiseModel::Isotropic::Sigma(1, 1e-0); + auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-0); + + auto f0 = std::make_shared>(X(1), X(2), 0.0, model0) + ->linearize(values); + auto f1 = std::make_shared>(X(1), X(2), 2.0, model1) + ->linearize(values); + std::vector factors{f0, f1}; + + GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors, true); + HybridGaussianFactorGraph hfg; + hfg.push_back(mixtureFactor); + + f0 = std::make_shared>(X(2), X(3), 0.0, model0) + ->linearize(values); + f1 = std::make_shared>(X(2), X(3), 2.0, model1) + ->linearize(values); + std::vector factors23{f0, f1}; + hfg.push_back(GaussianMixtureFactor({X(2), X(3)}, {m2}, factors23, true)); + + auto prior = PriorFactor(X(1), x1, prior_noise).linearize(values); + hfg.push_back(prior); + + hfg.push_back(PriorFactor(X(2), 2.0, prior_noise).linearize(values)); + + auto bn = hfg.eliminateSequential(); + HybridValues actual = bn->optimize(); + + HybridValues expected( + VectorValues{ + {X(1), Vector1(0.0)}, {X(2), Vector1(0.25)}, {X(3), Vector1(-0.6)}}, + DiscreteValues{{M(1), 1}, {M(2), 0}}); + + EXPECT(assert_equal(expected, actual)); + + { + DiscreteValues dv{{M(1), 0}, {M(2), 0}}; + VectorValues cont = bn->optimize(dv); + double error = bn->error(HybridValues(cont, dv)); + // regression + EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9); + } + { + DiscreteValues dv{{M(1), 0}, {M(2), 1}}; + VectorValues cont = bn->optimize(dv); + double error = bn->error(HybridValues(cont, dv)); + // regression + EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9); + } + { + DiscreteValues dv{{M(1), 1}, {M(2), 0}}; + VectorValues cont = bn->optimize(dv); + double error = bn->error(HybridValues(cont, dv)); + // regression + EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9); + } + { + DiscreteValues dv{{M(1), 1}, {M(2), 1}}; + VectorValues cont = bn->optimize(dv); + double error = bn->error(HybridValues(cont, dv)); + // regression + EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9); + } +} + +/* ************************************************************************* */ +/** + * @brief Test components with differing covariances. + * The factor graph is + * *-X1-*-X2 + * | + * M1 + */ +TEST(GaussianMixtureFactor, DifferentCovariances) { + DiscreteKey m1(M(1), 2); + + Values values; + double x1 = 1.0, x2 = 1.0; + values.insert(X(1), x1); + values.insert(X(2), x2); + + double between = 0.0; + + auto model0 = noiseModel::Isotropic::Sigma(1, 1e2); + auto model1 = noiseModel::Isotropic::Sigma(1, 1e-2); + auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); + + auto f0 = + std::make_shared>(X(1), X(2), between, model0); + auto f1 = + std::make_shared>(X(1), X(2), between, model1); + std::vector factors{f0, f1}; + + // Create via toFactorGraph + using symbol_shorthand::Z; + Matrix H0_1, H0_2, H1_1, H1_2; + Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2); + std::vector> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/}, + // + {X(1), H0_1 /*Sp1*/}, + {X(2), H0_2 /*Tp2*/}}; + + Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2); + std::vector> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/}, + // + {X(1), H1_1 /*Sp1*/}, + {X(2), H1_2 /*Tp2*/}}; + gtsam::GaussianMixtureFactor gmf( + {X(1), X(2)}, {m1}, + {std::make_shared(X(1), H0_1, X(2), H0_2, -d0, model0), + std::make_shared(X(1), H1_1, X(2), H1_2, -d1, model1)}, + true); + + // Create FG with single GaussianMixtureFactor + HybridGaussianFactorGraph mixture_fg; + mixture_fg.add(gmf); + + // Linearized prior factor on X1 + auto prior = PriorFactor(X(1), x1, prior_noise).linearize(values); + mixture_fg.push_back(prior); + + auto hbn = mixture_fg.eliminateSequential(); + // hbn->print(); + + VectorValues cv; + cv.insert(X(1), Vector1(0.0)); + cv.insert(X(2), Vector1(0.0)); + + // Check that the error values at the MLE point μ. + AlgebraicDecisionTree errorTree = hbn->errorTree(cv); + + DiscreteValues dv0{{M(1), 0}}; + DiscreteValues dv1{{M(1), 1}}; + + // regression + EXPECT_DOUBLES_EQUAL(0.69314718056, errorTree(dv0), 1e-9); + EXPECT_DOUBLES_EQUAL(0.69314718056, errorTree(dv1), 1e-9); + + DiscreteConditional expected_m1(m1, "0.5/0.5"); + DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete()); + + EXPECT(assert_equal(expected_m1, actual_m1)); } /* ************************************************************************* */ From d4e5a9be5d3b3fe6ec31e5238a8879d6ca77ccb7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 20 Aug 2024 07:57:15 -0400 Subject: [PATCH 42/71] different means test both via direct factor definition and toFactorGraph --- .../tests/testGaussianMixtureFactor.cpp | 79 ++++++++++++++++++- 1 file changed, 78 insertions(+), 1 deletion(-) diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 55ecce939..47b9ddc99 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -346,7 +346,84 @@ TEST(GaussianMixtureFactor, DifferentCovariances) { DiscreteValues dv1{{M(1), 1}}; // regression - EXPECT_DOUBLES_EQUAL(0.69314718056, errorTree(dv0), 1e-9); + EXPECT_DOUBLES_EQUAL(9.90348755254, errorTree(dv0), 1e-9); + EXPECT_DOUBLES_EQUAL(0.69314718056, errorTree(dv1), 1e-9); + + DiscreteConditional expected_m1(m1, "0.5/0.5"); + DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete()); + + EXPECT(assert_equal(expected_m1, actual_m1)); +} + +/* ************************************************************************* */ +/** + * @brief Test components with differing covariances + * but with a Bayes net P(Z|X, M) converted to a FG. + */ +TEST(GaussianMixtureFactor, DifferentCovariances2) { + DiscreteKey m1(M(1), 2); + + Values values; + double x1 = 1.0, x2 = 1.0; + values.insert(X(1), x1); + values.insert(X(2), x2); + + double between = 0.0; + + auto model0 = noiseModel::Isotropic::Sigma(1, 1e2); + auto model1 = noiseModel::Isotropic::Sigma(1, 1e-2); + auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); + + auto f0 = + std::make_shared>(X(1), X(2), between, model0); + auto f1 = + std::make_shared>(X(1), X(2), between, model1); + std::vector factors{f0, f1}; + + // Create via toFactorGraph + using symbol_shorthand::Z; + Matrix H0_1, H0_2, H1_1, H1_2; + Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2); + std::vector> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/}, + // + {X(1), H0_1 /*Sp1*/}, + {X(2), H0_2 /*Tp2*/}}; + + Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2); + std::vector> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/}, + // + {X(1), H1_1 /*Sp1*/}, + {X(2), H1_2 /*Tp2*/}}; + auto gm = new gtsam::GaussianMixture( + {Z(1)}, {X(1), X(2)}, {m1}, + {std::make_shared(terms0, 1, -d0, model0), + std::make_shared(terms1, 1, -d1, model1)}); + gtsam::HybridBayesNet bn; + bn.emplace_back(gm); + + gtsam::VectorValues measurements; + measurements.insert(Z(1), gtsam::Z_1x1); + // Create FG with single GaussianMixtureFactor + HybridGaussianFactorGraph mixture_fg = bn.toFactorGraph(measurements); + + // Linearized prior factor on X1 + auto prior = PriorFactor(X(1), x1, prior_noise).linearize(values); + mixture_fg.push_back(prior); + + auto hbn = mixture_fg.eliminateSequential(); + + VectorValues cv; + cv.insert(X(1), Vector1(0.0)); + cv.insert(X(2), Vector1(0.0)); + + // Check that the error values at the MLE point μ. + AlgebraicDecisionTree errorTree = hbn->errorTree(cv); + + DiscreteValues dv0{{M(1), 0}}; + DiscreteValues dv1{{M(1), 1}}; + + // regression + EXPECT_DOUBLES_EQUAL(9.90348755254, errorTree(dv0), 1e-9); EXPECT_DOUBLES_EQUAL(0.69314718056, errorTree(dv1), 1e-9); DiscreteConditional expected_m1(m1, "0.5/0.5"); From fef929f2664a1911226c17f47be47e456bad73af Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 20 Aug 2024 12:53:53 -0400 Subject: [PATCH 43/71] clean up model selection --- gtsam/hybrid/HybridBayesNet.cpp | 25 ++++--------------------- 1 file changed, 4 insertions(+), 21 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 23ee72215..29a850da8 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -284,15 +284,10 @@ GaussianBayesNetValTree HybridBayesNet::assembleTree() const { AlgebraicDecisionTree HybridBayesNet::modelSelection() const { /* To perform model selection, we need: - q(mu; M, Z) * sqrt((2*pi)^n*det(Sigma)) - - If q(mu; M, Z) = exp(-error) & k = 1.0 / sqrt((2*pi)^n*det(Sigma)) - thus, q * sqrt((2*pi)^n*det(Sigma)) = q/k = exp(log(q/k)) - = exp(log(q) - log(k)) = exp(-error - log(k)) - = exp(-(error + log(k))), + q(mu; M, Z) = exp(-error) where error is computed at the corresponding MAP point, gbn.error(mu). - So we compute (error + log(k)) and exponentiate later + So we compute `error` and exponentiate after. */ GaussianBayesNetValTree bnTree = assembleTree(); @@ -316,22 +311,10 @@ AlgebraicDecisionTree HybridBayesNet::modelSelection() const { return std::make_pair(gbnAndValue.first, error); }); + // Compute model selection term (with help from ADT methods) auto trees = unzip(bn_error); AlgebraicDecisionTree errorTree = trees.second; - - // Only compute logNormalizationConstant - AlgebraicDecisionTree log_norm_constants = DecisionTree( - bnTree, [](const std::pair &gbnAndValue) { - GaussianBayesNet gbn = gbnAndValue.first; - if (gbn.size() == 0) { - return 0.0; - } - return gbn.logNormalizationConstant(); - }); - - // Compute model selection term (with help from ADT methods) - AlgebraicDecisionTree modelSelectionTerm = - (errorTree + log_norm_constants) * -1; + AlgebraicDecisionTree modelSelectionTerm = errorTree * -1; double max_log = modelSelectionTerm.max(); modelSelectionTerm = DecisionTree( From 654bad738170185911a53ef551a64041a5975bf2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 20 Aug 2024 14:04:37 -0400 Subject: [PATCH 44/71] remove model selection code --- gtsam/hybrid/GaussianMixture.cpp | 51 ++++++------ gtsam/hybrid/HybridBayesNet.cpp | 134 ------------------------------- gtsam/hybrid/HybridBayesNet.h | 28 ------- gtsam/hybrid/HybridFactor.h | 7 +- 4 files changed, 27 insertions(+), 193 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 7f42e6986..8278549d0 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -71,29 +71,27 @@ GaussianMixture::GaussianMixture( Conditionals(discreteParents, conditionals)) {} /* *******************************************************************************/ -// TODO(dellaert): This is copy/paste: GaussianMixture should be derived from -// GaussianMixtureFactor, no? -GaussianFactorGraphTree GaussianMixture::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 GaussianMixture::asGaussianFactorGraphTree() const { +GaussianBayesNetTree GaussianMixture::asGaussianBayesNetTree() const { auto wrap = [](const GaussianConditional::shared_ptr &gc) { - return GaussianFactorGraph{gc}; + if (gc) { + return GaussianBayesNet{gc}; + } else { + return GaussianBayesNet(); + } }; return {conditionals_, wrap}; } /* *******************************************************************************/ +GaussianFactorGraphTree GaussianMixture::asGaussianFactorGraphTree() const { + auto wrap = [](const GaussianBayesNet &gbn) { + return GaussianFactorGraph(gbn); + }; + return {this->asGaussianBayesNetTree(), wrap}; +} + +/* +*******************************************************************************/ GaussianBayesNetTree GaussianMixture::add( const GaussianBayesNetTree &sum) const { using Y = GaussianBayesNet; @@ -110,15 +108,18 @@ GaussianBayesNetTree GaussianMixture::add( } /* *******************************************************************************/ -GaussianBayesNetTree GaussianMixture::asGaussianBayesNetTree() const { - auto wrap = [](const GaussianConditional::shared_ptr &gc) { - if (gc) { - return GaussianBayesNet{gc}; - } else { - return GaussianBayesNet(); - } +// TODO(dellaert): This is copy/paste: GaussianMixture should be derived from +// GaussianMixtureFactor, no? +GaussianFactorGraphTree GaussianMixture::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; }; - return {conditionals_, wrap}; + const auto tree = asGaussianFactorGraphTree(); + return sum.empty() ? tree : sum.apply(tree, add); } /* *******************************************************************************/ diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 29a850da8..1d01baed2 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -26,16 +26,6 @@ static std::mt19937_64 kRandomNumberGenerator(42); namespace gtsam { -/* ************************************************************************ */ -// Throw a runtime exception for method specified in string s, -// and conditional f: -static void throwRuntimeError(const std::string &s, - const std::shared_ptr &f) { - auto &fr = *f; - throw std::runtime_error(s + " not implemented for conditional type " + - demangle(typeid(fr).name()) + "."); -} - /* ************************************************************************* */ void HybridBayesNet::print(const std::string &s, const KeyFormatter &formatter) const { @@ -227,141 +217,17 @@ GaussianBayesNet HybridBayesNet::choose( return gbn; } -/* ************************************************************************ */ -static GaussianBayesNetTree addGaussian( - const GaussianBayesNetTree &gfgTree, - const GaussianConditional::shared_ptr &factor) { - // If the decision tree is not initialized, then initialize it. - if (gfgTree.empty()) { - GaussianBayesNet result{factor}; - return GaussianBayesNetTree(result); - } else { - auto add = [&factor](const GaussianBayesNet &graph) { - auto result = graph; - result.push_back(factor); - return result; - }; - return gfgTree.apply(add); - } -} - -/* ************************************************************************ */ -GaussianBayesNetValTree HybridBayesNet::assembleTree() const { - GaussianBayesNetTree result; - - for (auto &f : factors_) { - // TODO(dellaert): just use a virtual method defined in HybridFactor. - if (auto gm = std::dynamic_pointer_cast(f)) { - result = gm->add(result); - } else if (auto hc = std::dynamic_pointer_cast(f)) { - if (auto gm = hc->asMixture()) { - result = gm->add(result); - } else if (auto g = hc->asGaussian()) { - result = addGaussian(result, g); - } else { - // Has to be discrete. - // TODO(dellaert): in C++20, we can use std::visit. - continue; - } - } else if (std::dynamic_pointer_cast(f)) { - // Don't do anything for discrete-only factors - // since we want to evaluate continuous values only. - continue; - } else { - // We need to handle the case where the object is actually an - // BayesTreeOrphanWrapper! - throwRuntimeError("HybridBayesNet::assembleTree", f); - } - } - - GaussianBayesNetValTree resultTree(result, [](const GaussianBayesNet &gbn) { - return std::make_pair(gbn, 0.0); - }); - return resultTree; -} - -/* ************************************************************************* */ -AlgebraicDecisionTree HybridBayesNet::modelSelection() const { - /* - To perform model selection, we need: - q(mu; M, Z) = exp(-error) - where error is computed at the corresponding MAP point, gbn.error(mu). - - So we compute `error` and exponentiate after. - */ - - GaussianBayesNetValTree bnTree = assembleTree(); - - GaussianBayesNetValTree bn_error = bnTree.apply( - [this](const Assignment &assignment, - const std::pair &gbnAndValue) { - // Compute the X* of each assignment - VectorValues mu = gbnAndValue.first.optimize(); - - // mu is empty if gbn had nullptrs - if (mu.size() == 0) { - return std::make_pair(gbnAndValue.first, - std::numeric_limits::max()); - } - - // Compute the error for X* and the assignment - double error = - this->error(HybridValues(mu, DiscreteValues(assignment))); - - return std::make_pair(gbnAndValue.first, error); - }); - - // Compute model selection term (with help from ADT methods) - auto trees = unzip(bn_error); - AlgebraicDecisionTree errorTree = trees.second; - AlgebraicDecisionTree modelSelectionTerm = errorTree * -1; - - double max_log = modelSelectionTerm.max(); - modelSelectionTerm = DecisionTree( - modelSelectionTerm, - [&max_log](const double &x) { return std::exp(x - max_log); }); - modelSelectionTerm = modelSelectionTerm.normalize(modelSelectionTerm.sum()); - - return modelSelectionTerm; -} - /* ************************************************************************* */ HybridValues HybridBayesNet::optimize() const { // Collect all the discrete factors to compute MPE DiscreteFactorGraph discrete_fg; - // Compute model selection term - AlgebraicDecisionTree modelSelectionTerm = modelSelection(); - - // Get the set of all discrete keys involved in model selection - std::set discreteKeySet; for (auto &&conditional : *this) { if (conditional->isDiscrete()) { discrete_fg.push_back(conditional->asDiscrete()); - } else { - if (conditional->isContinuous()) { - /* - If we are here, it means there are no discrete variables in - the Bayes net (due to strong elimination ordering). - This is a continuous-only problem hence model selection doesn't matter. - */ - - } else if (conditional->isHybrid()) { - auto gm = conditional->asMixture(); - // Include the discrete keys - std::copy(gm->discreteKeys().begin(), gm->discreteKeys().end(), - std::inserter(discreteKeySet, discreteKeySet.end())); - } } } - // Only add model_selection if we have discrete keys - if (discreteKeySet.size() > 0) { - discrete_fg.push_back(DecisionTreeFactor( - DiscreteKeys(discreteKeySet.begin(), discreteKeySet.end()), - modelSelectionTerm)); - } - // Solve for the MPE DiscreteValues mpe = discrete_fg.optimize(); diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index fe9010b1f..032cd55b9 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -118,34 +118,6 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { return evaluate(values); } - /** - * @brief Assemble a DecisionTree of (GaussianBayesNet, double) leaves for - * each discrete assignment. - * The included double value is used to make - * constructing the model selection term cleaner and more efficient. - * - * @return GaussianBayesNetValTree - */ - GaussianBayesNetValTree assembleTree() const; - - /* - Compute L(M;Z), the likelihood of the discrete model M - given the measurements Z. - This is called the model selection term. - - To do so, we perform the integration of L(M;Z) ∝ L(X;M,Z)P(X|M). - - By Bayes' rule, P(X|M,Z) ∝ L(X;M,Z)P(X|M), - hence L(X;M,Z)P(X|M) is the unnormalized probabilty of - the joint Gaussian distribution. - - This can be computed by multiplying all the exponentiated errors - of each of the conditionals. - - Return a tree where each leaf value is L(M_i;Z). - */ - AlgebraicDecisionTree modelSelection() const; - /** * @brief Solve the HybridBayesNet by first computing the MPE of all the * discrete variables and then optimizing the continuous variables based on diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index 8828a9172..418489d66 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -13,6 +13,7 @@ * @file HybridFactor.h * @date Mar 11, 2022 * @author Fan Jiang + * @author Varun Agrawal */ #pragma once @@ -35,12 +36,6 @@ class HybridValues; using GaussianFactorGraphTree = DecisionTree; /// Alias for DecisionTree of GaussianBayesNets using GaussianBayesNetTree = DecisionTree; -/** - * Alias for DecisionTree of (GaussianBayesNet, double) pairs. - * Used for model selection in BayesNet::optimize - */ -using GaussianBayesNetValTree = - DecisionTree>; KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys); From 6b1d89d0a7ebb84db3c35f578820d1d3ae063838 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 20 Aug 2024 14:19:35 -0400 Subject: [PATCH 45/71] fix testMixtureFactor test --- gtsam/hybrid/tests/testMixtureFactor.cpp | 26 ++++++------------------ 1 file changed, 6 insertions(+), 20 deletions(-) diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp index c03df4d61..58a12b3d8 100644 --- a/gtsam/hybrid/tests/testMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testMixtureFactor.cpp @@ -252,30 +252,16 @@ TEST(MixtureFactor, DifferentCovariances) { // Check that we get different error values at the MLE point μ. AlgebraicDecisionTree errorTree = hbn->errorTree(cv); - auto cond0 = hbn->at(0)->asMixture(); - auto cond1 = hbn->at(1)->asMixture(); - auto discrete_cond = hbn->at(2)->asDiscrete(); HybridValues hv0(cv, DiscreteValues{{M(1), 0}}); HybridValues hv1(cv, DiscreteValues{{M(1), 1}}); - AlgebraicDecisionTree expectedErrorTree( - m1, - cond0->error(hv0) // cond0(0)->logNormalizationConstant() - // - cond0(1)->logNormalizationConstant - + cond1->error(hv0) + discrete_cond->error(DiscreteValues{{M(1), 0}}), - cond0->error(hv1) // cond1(0)->logNormalizationConstant() - // - cond1(1)->logNormalizationConstant - + cond1->error(hv1) + - discrete_cond->error(DiscreteValues{{M(1), 0}})); + + auto cond0 = hbn->at(0)->asMixture(); + auto cond1 = hbn->at(1)->asMixture(); + auto discrete_cond = hbn->at(2)->asDiscrete(); + AlgebraicDecisionTree expectedErrorTree(m1, 9.90348755254, + 0.69314718056); EXPECT(assert_equal(expectedErrorTree, errorTree)); - - DiscreteValues dv; - dv.insert({M(1), 1}); - HybridValues expected_values(cv, dv); - - HybridValues actual_values = hbn->optimize(); - - EXPECT(assert_equal(expected_values, actual_values)); } /* ************************************************************************* */ From 6d9fc8e5f21b020e5dc8f250912d973d243ee665 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 20 Aug 2024 15:26:04 -0400 Subject: [PATCH 46/71] undo change in GaussianMixture --- gtsam/hybrid/GaussianMixture.cpp | 44 ++++++++++++++++---------------- 1 file changed, 22 insertions(+), 22 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 8278549d0..36a34226b 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -71,23 +71,26 @@ GaussianMixture::GaussianMixture( Conditionals(discreteParents, conditionals)) {} /* *******************************************************************************/ -GaussianBayesNetTree GaussianMixture::asGaussianBayesNetTree() const { - auto wrap = [](const GaussianConditional::shared_ptr &gc) { - if (gc) { - return GaussianBayesNet{gc}; - } else { - return GaussianBayesNet(); - } +// TODO(dellaert): This is copy/paste: GaussianMixture should be derived from +// GaussianMixtureFactor, no? +GaussianFactorGraphTree GaussianMixture::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; }; - return {conditionals_, wrap}; + const auto tree = asGaussianFactorGraphTree(); + return sum.empty() ? tree : sum.apply(tree, add); } /* *******************************************************************************/ GaussianFactorGraphTree GaussianMixture::asGaussianFactorGraphTree() const { - auto wrap = [](const GaussianBayesNet &gbn) { - return GaussianFactorGraph(gbn); + auto wrap = [](const GaussianConditional::shared_ptr &gc) { + return GaussianFactorGraph{gc}; }; - return {this->asGaussianBayesNetTree(), wrap}; + return {conditionals_, wrap}; } /* @@ -108,18 +111,15 @@ GaussianBayesNetTree GaussianMixture::add( } /* *******************************************************************************/ -// TODO(dellaert): This is copy/paste: GaussianMixture should be derived from -// GaussianMixtureFactor, no? -GaussianFactorGraphTree GaussianMixture::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; +GaussianBayesNetTree GaussianMixture::asGaussianBayesNetTree() const { + auto wrap = [](const GaussianConditional::shared_ptr &gc) { + if (gc) { + return GaussianBayesNet{gc}; + } else { + return GaussianBayesNet(); + } }; - const auto tree = asGaussianFactorGraphTree(); - return sum.empty() ? tree : sum.apply(tree, add); + return {conditionals_, wrap}; } /* *******************************************************************************/ From fd2062b2079a319a5a70c058aacc0adb3f252065 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 20 Aug 2024 15:48:07 -0400 Subject: [PATCH 47/71] remove changes so we can break up PR into smaller ones --- gtsam/hybrid/GaussianMixture.cpp | 67 +---- gtsam/hybrid/GaussianMixture.h | 18 +- gtsam/hybrid/GaussianMixtureFactor.cpp | 83 +----- gtsam/hybrid/GaussianMixtureFactor.h | 18 +- gtsam/hybrid/HybridBayesNet.cpp | 6 +- gtsam/hybrid/HybridFactor.h | 3 - gtsam/hybrid/HybridGaussianFactorGraph.cpp | 40 +-- .../tests/testGaussianMixtureFactor.cpp | 247 +----------------- .../tests/testHybridNonlinearFactorGraph.cpp | 15 -- gtsam/hybrid/tests/testMixtureFactor.cpp | 149 ----------- gtsam/linear/GaussianBayesNet.cpp | 20 -- gtsam/linear/GaussianBayesNet.h | 14 - gtsam/linear/GaussianConditional.cpp | 10 +- gtsam/linear/VectorValues.h | 5 - gtsam/linear/linear.i | 5 - .../linear/tests/testGaussianConditional.cpp | 3 - 16 files changed, 37 insertions(+), 666 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 36a34226b..531a9f2e5 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -24,7 +24,6 @@ #include #include #include -#include #include namespace gtsam { @@ -93,35 +92,6 @@ GaussianFactorGraphTree GaussianMixture::asGaussianFactorGraphTree() const { return {conditionals_, wrap}; } -/* -*******************************************************************************/ -GaussianBayesNetTree GaussianMixture::add( - const GaussianBayesNetTree &sum) const { - using Y = GaussianBayesNet; - auto add = [](const Y &graph1, const Y &graph2) { - auto result = graph1; - if (graph2.size() == 0) { - return GaussianBayesNet(); - } - result.push_back(graph2); - return result; - }; - const auto tree = asGaussianBayesNetTree(); - return sum.empty() ? tree : sum.apply(tree, add); -} - -/* *******************************************************************************/ -GaussianBayesNetTree GaussianMixture::asGaussianBayesNetTree() const { - auto wrap = [](const GaussianConditional::shared_ptr &gc) { - if (gc) { - return GaussianBayesNet{gc}; - } else { - return GaussianBayesNet(); - } - }; - return {conditionals_, wrap}; -} - /* *******************************************************************************/ size_t GaussianMixture::nrComponents() const { size_t total = 0; @@ -348,15 +318,8 @@ AlgebraicDecisionTree GaussianMixture::logProbability( AlgebraicDecisionTree GaussianMixture::errorTree( const VectorValues &continuousValues) const { auto errorFunc = [&](const GaussianConditional::shared_ptr &conditional) { - // Check if valid pointer - if (conditional) { - return conditional->error(continuousValues) + // - logConstant_ - conditional->logNormalizationConstant(); - } else { - // If not valid, pointer, it means this conditional was pruned, - // so we return maximum error. - return std::numeric_limits::max(); - } + return conditional->error(continuousValues) + // + logConstant_ - conditional->logNormalizationConstant(); }; DecisionTree error_tree(conditionals_, errorFunc); return error_tree; @@ -364,32 +327,10 @@ AlgebraicDecisionTree GaussianMixture::errorTree( /* *******************************************************************************/ double GaussianMixture::error(const HybridValues &values) const { - // Check if discrete keys in discrete assignment are - // present in the GaussianMixture - KeyVector dKeys = this->discreteKeys_.indices(); - bool valid_assignment = false; - for (auto &&kv : values.discrete()) { - if (std::find(dKeys.begin(), dKeys.end(), kv.first) != dKeys.end()) { - valid_assignment = true; - break; - } - } - - // The discrete assignment is not valid so we return 0.0 erorr. - if (!valid_assignment) { - return 0.0; - } - // Directly index to get the conditional, no need to build the whole tree. auto conditional = conditionals_(values.discrete()); - if (conditional) { - return conditional->error(values.continuous()) + // - logConstant_ - conditional->logNormalizationConstant(); - } else { - // If not valid, pointer, it means this conditional was pruned, - // so we return maximum error. - return std::numeric_limits::max(); - } + return conditional->error(values.continuous()) + // + logConstant_ - conditional->logNormalizationConstant(); } /* *******************************************************************************/ diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index cefc14241..c1ef504f8 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -72,12 +72,6 @@ class GTSAM_EXPORT GaussianMixture */ GaussianFactorGraphTree asGaussianFactorGraphTree() const; - /** - * @brief Convert a DecisionTree of conditionals into - * a DecisionTree of Gaussian Bayes nets. - */ - GaussianBayesNetTree asGaussianBayesNetTree() const; - /** * @brief Helper function to get the pruner functor. * @@ -221,7 +215,8 @@ class GTSAM_EXPORT GaussianMixture * @return AlgebraicDecisionTree A decision tree on the discrete keys * only, with the leaf values as the error for each assignment. */ - AlgebraicDecisionTree errorTree(const VectorValues &continuousValues) const; + AlgebraicDecisionTree errorTree( + const VectorValues &continuousValues) const; /** * @brief Compute the logProbability of this Gaussian Mixture. @@ -255,15 +250,6 @@ class GTSAM_EXPORT GaussianMixture * @return GaussianFactorGraphTree */ GaussianFactorGraphTree add(const GaussianFactorGraphTree &sum) const; - - /** - * @brief Merge the Gaussian Bayes Nets in `this` and `sum` while - * maintaining the decision tree structure. - * - * @param sum Decision Tree of Gaussian Bayes Nets - * @return GaussianBayesNetTree - */ - GaussianBayesNetTree add(const GaussianBayesNetTree &sum) const; /// @} private: diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index e519cefe6..a3db16d04 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -28,86 +28,11 @@ namespace gtsam { -/** - * @brief Helper function to correct the [A|b] matrices in the factor components - * with the normalizer values. - * This is done by storing the normalizer value in - * the `b` vector as an additional row. - * - * @param factors DecisionTree of GaussianFactor shared pointers. - * @param varyingNormalizers Flag indicating the normalizers are different for - * each component. - * @return GaussianMixtureFactor::Factors - */ -GaussianMixtureFactor::Factors correct( - const GaussianMixtureFactor::Factors &factors, bool varyingNormalizers) { - if (!varyingNormalizers) { - return factors; - } - - // First compute all the sqrt(|2 pi Sigma|) terms - auto computeNormalizers = [](const GaussianMixtureFactor::sharedFactor &gf) { - auto jf = std::dynamic_pointer_cast(gf); - // If we have, say, a Hessian factor, then no need to do anything - if (!jf) return 0.0; - - auto model = jf->get_model(); - // If there is no noise model, there is nothing to do. - if (!model) { - return 0.0; - } - // Since noise models are Gaussian, we can get the logDeterminant using the - // same trick as in GaussianConditional - double logDetR = - model->R().diagonal().unaryExpr([](double x) { return log(x); }).sum(); - double logDeterminantSigma = -2.0 * logDetR; - - size_t n = model->dim(); - constexpr double log2pi = 1.8378770664093454835606594728112; - return n * log2pi + logDeterminantSigma; - }; - - AlgebraicDecisionTree log_normalizers = - DecisionTree(factors, computeNormalizers); - - // Find the minimum value so we can "proselytize" to positive values. - // Done because we can't have sqrt of negative numbers. - double min_log_normalizer = log_normalizers.min(); - log_normalizers = log_normalizers.apply( - [&min_log_normalizer](double n) { return n - min_log_normalizer; }); - - // Finally, update the [A|b] matrices. - auto update = [&log_normalizers]( - const Assignment &assignment, - const GaussianMixtureFactor::sharedFactor &gf) { - auto jf = std::dynamic_pointer_cast(gf); - if (!jf) return gf; - // If there is no noise model, there is nothing to do. - if (!jf->get_model()) return gf; - // If the log_normalizer is 0, do nothing - if (log_normalizers(assignment) == 0.0) return gf; - - GaussianFactorGraph gfg; - gfg.push_back(jf); - - Vector c(1); - c << std::sqrt(log_normalizers(assignment)); - auto constantFactor = std::make_shared(c); - - gfg.push_back(constantFactor); - return std::dynamic_pointer_cast( - std::make_shared(gfg)); - }; - return factors.apply(update); -} - /* *******************************************************************************/ GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, - const Factors &factors, - bool varyingNormalizers) - : Base(continuousKeys, discreteKeys), - factors_(correct(factors, varyingNormalizers)) {} + const Factors &factors) + : Base(continuousKeys, discreteKeys), factors_(factors) {} /* *******************************************************************************/ bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const { @@ -129,9 +54,7 @@ bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const { /* *******************************************************************************/ void GaussianMixtureFactor::print(const std::string &s, const KeyFormatter &formatter) const { - std::cout << (s.empty() ? "" : s + "\n"); - std::cout << "GaussianMixtureFactor" << std::endl; - HybridFactor::print("", formatter); + HybridFactor::print(s, formatter); std::cout << "{\n"; if (factors_.empty()) { std::cout << " empty" << std::endl; diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index 588501bbe..67d12ddb0 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -82,13 +82,10 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { * their cardinalities. * @param factors The decision tree of Gaussian factors stored as the mixture * density. - * @param varyingNormalizers Flag indicating factor components have varying - * normalizer values. */ GaussianMixtureFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, - const Factors &factors, - bool varyingNormalizers = false); + const Factors &factors); /** * @brief Construct a new GaussianMixtureFactor object using a vector of @@ -97,16 +94,12 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { * @param continuousKeys Vector of keys for continuous factors. * @param discreteKeys Vector of discrete keys. * @param factors Vector of gaussian factor shared pointers. - * @param varyingNormalizers Flag indicating factor components have varying - * normalizer values. */ GaussianMixtureFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, - const std::vector &factors, - bool varyingNormalizers = false) + const std::vector &factors) : GaussianMixtureFactor(continuousKeys, discreteKeys, - Factors(discreteKeys, factors), - varyingNormalizers) {} + Factors(discreteKeys, factors)) {} /// @} /// @name Testable @@ -114,8 +107,9 @@ class GTSAM_EXPORT GaussianMixtureFactor : 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 = "GaussianMixtureFactor\n", + const KeyFormatter &formatter = DefaultKeyFormatter) const override; /// @} /// @name Standard API diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 1d01baed2..bb0f23686 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -220,16 +220,16 @@ GaussianBayesNet HybridBayesNet::choose( /* ************************************************************************* */ HybridValues HybridBayesNet::optimize() const { // Collect all the discrete factors to compute MPE - DiscreteFactorGraph discrete_fg; + DiscreteBayesNet discrete_bn; for (auto &&conditional : *this) { if (conditional->isDiscrete()) { - discrete_fg.push_back(conditional->asDiscrete()); + discrete_bn.push_back(conditional->asDiscrete()); } } // Solve for the MPE - DiscreteValues mpe = discrete_fg.optimize(); + DiscreteValues mpe = DiscreteFactorGraph(discrete_bn).optimize(); // Given the MPE, compute the optimal continuous values. return HybridValues(optimize(mpe), mpe); diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index 418489d66..afd1c8032 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -13,7 +13,6 @@ * @file HybridFactor.h * @date Mar 11, 2022 * @author Fan Jiang - * @author Varun Agrawal */ #pragma once @@ -34,8 +33,6 @@ class HybridValues; /// Alias for DecisionTree of GaussianFactorGraphs using GaussianFactorGraphTree = DecisionTree; -/// Alias for DecisionTree of GaussianBayesNets -using GaussianBayesNetTree = DecisionTree; KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys); diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index f05dfd423..b764dc9e0 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -279,37 +279,21 @@ GaussianFactorGraphTree removeEmpty(const GaussianFactorGraphTree &sum) { using Result = std::pair, GaussianMixtureFactor::sharedFactor>; -/** - * Compute the probability q(μ;m) = exp(-error(μ;m)) * sqrt(det(2π Σ_m) - * from the residual error at the mean μ. - * The residual error contains no keys, and only - * depends on the discrete separator if present. - */ +// Integrate the probability mass in the last continuous conditional using +// the unnormalized probability q(μ;m) = exp(-error(μ;m)) at the mean. +// discrete_probability = exp(-error(μ;m)) * sqrt(det(2π Σ_m)) static std::shared_ptr createDiscreteFactor( const DecisionTree &eliminationResults, const DiscreteKeys &discreteSeparator) { - auto logProbability = [&](const Result &pair) -> double { + auto probability = [&](const Result &pair) -> double { const auto &[conditional, factor] = pair; static const VectorValues kEmpty; // If the factor is not null, it has no keys, just contains the residual. if (!factor) return 1.0; // TODO(dellaert): not loving this. - - // Logspace version of: - // exp(-factor->error(kEmpty)) / conditional->normalizationConstant(); - // We take negative of the logNormalizationConstant `log(1/k)` - // to get `log(k)`. - return -factor->error(kEmpty) + (-conditional->logNormalizationConstant()); + return exp(-factor->error(kEmpty)) / conditional->normalizationConstant(); }; - AlgebraicDecisionTree logProbabilities( - DecisionTree(eliminationResults, logProbability)); - - // Perform normalization - double max_log = logProbabilities.max(); - AlgebraicDecisionTree probabilities = DecisionTree( - logProbabilities, - [&max_log](const double x) { return exp(x - max_log); }); - probabilities = probabilities.normalize(probabilities.sum()); + DecisionTree probabilities(eliminationResults, probability); return std::make_shared(discreteSeparator, probabilities); } @@ -370,12 +354,6 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // Perform elimination! DecisionTree eliminationResults(factorGraphTree, eliminate); - // Create the GaussianMixture from the conditionals - GaussianMixture::Conditionals conditionals( - eliminationResults, [](const Result &pair) { return pair.first; }); - auto gaussianMixture = std::make_shared( - frontalKeys, continuousSeparator, discreteSeparator, conditionals); - // If there are no more continuous parents we create a DiscreteFactor with the // error for each discrete choice. Otherwise, create a GaussianMixtureFactor // on the separator, taking care to correct for conditional constants. @@ -385,6 +363,12 @@ hybridElimination(const HybridGaussianFactorGraph &factors, : createGaussianMixtureFactor(eliminationResults, continuousSeparator, discreteSeparator); + // Create the GaussianMixture from the conditionals + GaussianMixture::Conditionals conditionals( + eliminationResults, [](const Result &pair) { return pair.first; }); + auto gaussianMixture = std::make_shared( + frontalKeys, continuousSeparator, discreteSeparator, conditionals); + return {std::make_shared(gaussianMixture), newFactor}; } diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 47b9ddc99..9cc7e6bfd 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -22,13 +22,9 @@ #include #include #include -#include -#include #include #include #include -#include -#include // Include for test suite #include @@ -60,6 +56,7 @@ TEST(GaussianMixtureFactor, Sum) { auto b = Matrix::Zero(2, 1); Vector2 sigmas; sigmas << 1, 2; + auto model = noiseModel::Diagonal::Sigmas(sigmas, true); auto f10 = std::make_shared(X(1), A1, X(2), A2, b); auto f11 = std::make_shared(X(1), A1, X(2), A2, b); @@ -109,8 +106,7 @@ TEST(GaussianMixtureFactor, Printing) { GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors); std::string expected = - R"(GaussianMixtureFactor -Hybrid [x1 x2; 1]{ + R"(Hybrid [x1 x2; 1]{ Choice(1) 0 Leaf : A[x1] = [ @@ -182,8 +178,7 @@ TEST(GaussianMixtureFactor, Error) { continuousValues.insert(X(2), Vector2(1, 1)); // error should return a tree of errors, with nodes for each discrete value. - AlgebraicDecisionTree error_tree = - mixtureFactor.errorTree(continuousValues); + AlgebraicDecisionTree error_tree = mixtureFactor.errorTree(continuousValues); std::vector discrete_keys = {m1}; // Error values for regression test @@ -196,240 +191,8 @@ TEST(GaussianMixtureFactor, Error) { DiscreteValues discreteValues; discreteValues[m1.first] = 1; EXPECT_DOUBLES_EQUAL( - 4.0, mixtureFactor.error({continuousValues, discreteValues}), 1e-9); -} - -/* ************************************************************************* */ -// Test components with differing means -TEST(GaussianMixtureFactor, DifferentMeans) { - DiscreteKey m1(M(1), 2), m2(M(2), 2); - - Values values; - double x1 = 0.0, x2 = 1.75, x3 = 2.60; - values.insert(X(1), x1); - values.insert(X(2), x2); - values.insert(X(3), x3); - - auto model0 = noiseModel::Isotropic::Sigma(1, 1e-0); - auto model1 = noiseModel::Isotropic::Sigma(1, 1e-0); - auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-0); - - auto f0 = std::make_shared>(X(1), X(2), 0.0, model0) - ->linearize(values); - auto f1 = std::make_shared>(X(1), X(2), 2.0, model1) - ->linearize(values); - std::vector factors{f0, f1}; - - GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors, true); - HybridGaussianFactorGraph hfg; - hfg.push_back(mixtureFactor); - - f0 = std::make_shared>(X(2), X(3), 0.0, model0) - ->linearize(values); - f1 = std::make_shared>(X(2), X(3), 2.0, model1) - ->linearize(values); - std::vector factors23{f0, f1}; - hfg.push_back(GaussianMixtureFactor({X(2), X(3)}, {m2}, factors23, true)); - - auto prior = PriorFactor(X(1), x1, prior_noise).linearize(values); - hfg.push_back(prior); - - hfg.push_back(PriorFactor(X(2), 2.0, prior_noise).linearize(values)); - - auto bn = hfg.eliminateSequential(); - HybridValues actual = bn->optimize(); - - HybridValues expected( - VectorValues{ - {X(1), Vector1(0.0)}, {X(2), Vector1(0.25)}, {X(3), Vector1(-0.6)}}, - DiscreteValues{{M(1), 1}, {M(2), 0}}); - - EXPECT(assert_equal(expected, actual)); - - { - DiscreteValues dv{{M(1), 0}, {M(2), 0}}; - VectorValues cont = bn->optimize(dv); - double error = bn->error(HybridValues(cont, dv)); - // regression - EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9); - } - { - DiscreteValues dv{{M(1), 0}, {M(2), 1}}; - VectorValues cont = bn->optimize(dv); - double error = bn->error(HybridValues(cont, dv)); - // regression - EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9); - } - { - DiscreteValues dv{{M(1), 1}, {M(2), 0}}; - VectorValues cont = bn->optimize(dv); - double error = bn->error(HybridValues(cont, dv)); - // regression - EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9); - } - { - DiscreteValues dv{{M(1), 1}, {M(2), 1}}; - VectorValues cont = bn->optimize(dv); - double error = bn->error(HybridValues(cont, dv)); - // regression - EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9); - } -} - -/* ************************************************************************* */ -/** - * @brief Test components with differing covariances. - * The factor graph is - * *-X1-*-X2 - * | - * M1 - */ -TEST(GaussianMixtureFactor, DifferentCovariances) { - DiscreteKey m1(M(1), 2); - - Values values; - double x1 = 1.0, x2 = 1.0; - values.insert(X(1), x1); - values.insert(X(2), x2); - - double between = 0.0; - - auto model0 = noiseModel::Isotropic::Sigma(1, 1e2); - auto model1 = noiseModel::Isotropic::Sigma(1, 1e-2); - auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); - - auto f0 = - std::make_shared>(X(1), X(2), between, model0); - auto f1 = - std::make_shared>(X(1), X(2), between, model1); - std::vector factors{f0, f1}; - - // Create via toFactorGraph - using symbol_shorthand::Z; - Matrix H0_1, H0_2, H1_1, H1_2; - Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2); - std::vector> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/}, - // - {X(1), H0_1 /*Sp1*/}, - {X(2), H0_2 /*Tp2*/}}; - - Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2); - std::vector> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/}, - // - {X(1), H1_1 /*Sp1*/}, - {X(2), H1_2 /*Tp2*/}}; - gtsam::GaussianMixtureFactor gmf( - {X(1), X(2)}, {m1}, - {std::make_shared(X(1), H0_1, X(2), H0_2, -d0, model0), - std::make_shared(X(1), H1_1, X(2), H1_2, -d1, model1)}, - true); - - // Create FG with single GaussianMixtureFactor - HybridGaussianFactorGraph mixture_fg; - mixture_fg.add(gmf); - - // Linearized prior factor on X1 - auto prior = PriorFactor(X(1), x1, prior_noise).linearize(values); - mixture_fg.push_back(prior); - - auto hbn = mixture_fg.eliminateSequential(); - // hbn->print(); - - VectorValues cv; - cv.insert(X(1), Vector1(0.0)); - cv.insert(X(2), Vector1(0.0)); - - // Check that the error values at the MLE point μ. - AlgebraicDecisionTree errorTree = hbn->errorTree(cv); - - DiscreteValues dv0{{M(1), 0}}; - DiscreteValues dv1{{M(1), 1}}; - - // regression - EXPECT_DOUBLES_EQUAL(9.90348755254, errorTree(dv0), 1e-9); - EXPECT_DOUBLES_EQUAL(0.69314718056, errorTree(dv1), 1e-9); - - DiscreteConditional expected_m1(m1, "0.5/0.5"); - DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete()); - - EXPECT(assert_equal(expected_m1, actual_m1)); -} - -/* ************************************************************************* */ -/** - * @brief Test components with differing covariances - * but with a Bayes net P(Z|X, M) converted to a FG. - */ -TEST(GaussianMixtureFactor, DifferentCovariances2) { - DiscreteKey m1(M(1), 2); - - Values values; - double x1 = 1.0, x2 = 1.0; - values.insert(X(1), x1); - values.insert(X(2), x2); - - double between = 0.0; - - auto model0 = noiseModel::Isotropic::Sigma(1, 1e2); - auto model1 = noiseModel::Isotropic::Sigma(1, 1e-2); - auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); - - auto f0 = - std::make_shared>(X(1), X(2), between, model0); - auto f1 = - std::make_shared>(X(1), X(2), between, model1); - std::vector factors{f0, f1}; - - // Create via toFactorGraph - using symbol_shorthand::Z; - Matrix H0_1, H0_2, H1_1, H1_2; - Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2); - std::vector> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/}, - // - {X(1), H0_1 /*Sp1*/}, - {X(2), H0_2 /*Tp2*/}}; - - Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2); - std::vector> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/}, - // - {X(1), H1_1 /*Sp1*/}, - {X(2), H1_2 /*Tp2*/}}; - auto gm = new gtsam::GaussianMixture( - {Z(1)}, {X(1), X(2)}, {m1}, - {std::make_shared(terms0, 1, -d0, model0), - std::make_shared(terms1, 1, -d1, model1)}); - gtsam::HybridBayesNet bn; - bn.emplace_back(gm); - - gtsam::VectorValues measurements; - measurements.insert(Z(1), gtsam::Z_1x1); - // Create FG with single GaussianMixtureFactor - HybridGaussianFactorGraph mixture_fg = bn.toFactorGraph(measurements); - - // Linearized prior factor on X1 - auto prior = PriorFactor(X(1), x1, prior_noise).linearize(values); - mixture_fg.push_back(prior); - - auto hbn = mixture_fg.eliminateSequential(); - - VectorValues cv; - cv.insert(X(1), Vector1(0.0)); - cv.insert(X(2), Vector1(0.0)); - - // Check that the error values at the MLE point μ. - AlgebraicDecisionTree errorTree = hbn->errorTree(cv); - - DiscreteValues dv0{{M(1), 0}}; - DiscreteValues dv1{{M(1), 1}}; - - // regression - EXPECT_DOUBLES_EQUAL(9.90348755254, errorTree(dv0), 1e-9); - EXPECT_DOUBLES_EQUAL(0.69314718056, errorTree(dv1), 1e-9); - - DiscreteConditional expected_m1(m1, "0.5/0.5"); - DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete()); - - EXPECT(assert_equal(expected_m1, actual_m1)); + 4.0, mixtureFactor.error({continuousValues, discreteValues}), + 1e-9); } /* ************************************************************************* */ diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 2d851b0ff..cf56e35d6 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -510,7 +510,6 @@ factor 0: b = [ -10 ] No noise model factor 1: -GaussianMixtureFactor Hybrid [x0 x1; m0]{ Choice(m0) 0 Leaf : @@ -535,7 +534,6 @@ Hybrid [x0 x1; m0]{ } factor 2: -GaussianMixtureFactor Hybrid [x1 x2; m1]{ Choice(m1) 0 Leaf : @@ -677,26 +675,22 @@ factor 6: P( m1 | m0 ): size: 3 conditional 0: Hybrid P( x0 | x1 m0) Discrete Keys = (m0, 2), - logNormalizationConstant: 1.38862 Choice(m0) 0 Leaf p(x0 | x1) R = [ 10.0499 ] S[x1] = [ -0.0995037 ] d = [ -9.85087 ] - logNormalizationConstant: 1.38862 No noise model 1 Leaf p(x0 | x1) R = [ 10.0499 ] S[x1] = [ -0.0995037 ] d = [ -9.95037 ] - logNormalizationConstant: 1.38862 No noise model conditional 1: Hybrid P( x1 | x2 m0 m1) Discrete Keys = (m0, 2), (m1, 2), - logNormalizationConstant: 1.3935 Choice(m1) 0 Choice(m0) @@ -704,14 +698,12 @@ conditional 1: Hybrid P( x1 | x2 m0 m1) R = [ 10.099 ] S[x2] = [ -0.0990196 ] d = [ -9.99901 ] - logNormalizationConstant: 1.3935 No noise model 0 1 Leaf p(x1 | x2) R = [ 10.099 ] S[x2] = [ -0.0990196 ] d = [ -9.90098 ] - logNormalizationConstant: 1.3935 No noise model 1 Choice(m0) @@ -719,19 +711,16 @@ conditional 1: Hybrid P( x1 | x2 m0 m1) R = [ 10.099 ] S[x2] = [ -0.0990196 ] d = [ -10.098 ] - logNormalizationConstant: 1.3935 No noise model 1 1 Leaf p(x1 | x2) R = [ 10.099 ] S[x2] = [ -0.0990196 ] d = [ -10 ] - logNormalizationConstant: 1.3935 No noise model conditional 2: Hybrid P( x2 | m0 m1) Discrete Keys = (m0, 2), (m1, 2), - logNormalizationConstant: 1.38857 Choice(m1) 0 Choice(m0) @@ -740,7 +729,6 @@ conditional 2: Hybrid P( x2 | m0 m1) d = [ -10.1489 ] mean: 1 elements x2: -1.0099 - logNormalizationConstant: 1.38857 No noise model 0 1 Leaf p(x2) @@ -748,7 +736,6 @@ conditional 2: Hybrid P( x2 | m0 m1) d = [ -10.1479 ] mean: 1 elements x2: -1.0098 - logNormalizationConstant: 1.38857 No noise model 1 Choice(m0) @@ -757,7 +744,6 @@ conditional 2: Hybrid P( x2 | m0 m1) d = [ -10.0504 ] mean: 1 elements x2: -1.0001 - logNormalizationConstant: 1.38857 No noise model 1 1 Leaf p(x2) @@ -765,7 +751,6 @@ conditional 2: Hybrid P( x2 | m0 m1) d = [ -10.0494 ] mean: 1 elements x2: -1 - logNormalizationConstant: 1.38857 No noise model )"; diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp index 58a12b3d8..0b2564403 100644 --- a/gtsam/hybrid/tests/testMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testMixtureFactor.cpp @@ -18,9 +18,6 @@ #include #include -#include -#include -#include #include #include #include @@ -118,152 +115,6 @@ TEST(MixtureFactor, Dim) { EXPECT_LONGS_EQUAL(1, mixtureFactor.dim()); } -/* ************************************************************************* */ -// Test components with differing means -TEST(MixtureFactor, DifferentMeans) { - DiscreteKey m1(M(1), 2), m2(M(2), 2); - - Values values; - double x1 = 0.0, x2 = 1.75, x3 = 2.60; - values.insert(X(1), x1); - values.insert(X(2), x2); - values.insert(X(3), x3); - - auto model0 = noiseModel::Isotropic::Sigma(1, 1e-0); - auto model1 = noiseModel::Isotropic::Sigma(1, 1e-0); - auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-0); - - auto f0 = std::make_shared>(X(1), X(2), 0.0, model0); - auto f1 = std::make_shared>(X(1), X(2), 2.0, model1); - std::vector factors{f0, f1}; - - MixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors); - HybridNonlinearFactorGraph hnfg; - hnfg.push_back(mixtureFactor); - - f0 = std::make_shared>(X(2), X(3), 0.0, model0); - f1 = std::make_shared>(X(2), X(3), 2.0, model1); - std::vector factors23{f0, f1}; - hnfg.push_back(MixtureFactor({X(2), X(3)}, {m2}, factors23)); - - auto prior = PriorFactor(X(1), x1, prior_noise); - hnfg.push_back(prior); - - hnfg.emplace_shared>(X(2), 2.0, prior_noise); - - auto hgfg = hnfg.linearize(values); - auto bn = hgfg->eliminateSequential(); - HybridValues actual = bn->optimize(); - - HybridValues expected( - VectorValues{ - {X(1), Vector1(0.0)}, {X(2), Vector1(0.25)}, {X(3), Vector1(-0.6)}}, - DiscreteValues{{M(1), 1}, {M(2), 0}}); - - EXPECT(assert_equal(expected, actual)); - - { - DiscreteValues dv{{M(1), 0}, {M(2), 0}}; - VectorValues cont = bn->optimize(dv); - double error = bn->error(HybridValues(cont, dv)); - // regression - EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9); - } - { - DiscreteValues dv{{M(1), 0}, {M(2), 1}}; - VectorValues cont = bn->optimize(dv); - double error = bn->error(HybridValues(cont, dv)); - // regression - EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9); - } - { - DiscreteValues dv{{M(1), 1}, {M(2), 0}}; - VectorValues cont = bn->optimize(dv); - double error = bn->error(HybridValues(cont, dv)); - // regression - EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9); - } - { - DiscreteValues dv{{M(1), 1}, {M(2), 1}}; - VectorValues cont = bn->optimize(dv); - double error = bn->error(HybridValues(cont, dv)); - // regression - EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9); - } -} - -/* ************************************************************************* */ -// Test components with differing covariances -TEST(MixtureFactor, DifferentCovariances) { - DiscreteKey m1(M(1), 2); - - Values values; - double x1 = 1.0, x2 = 1.0; - values.insert(X(1), x1); - values.insert(X(2), x2); - - double between = 0.0; - - auto model0 = noiseModel::Isotropic::Sigma(1, 1e2); - auto model1 = noiseModel::Isotropic::Sigma(1, 1e-2); - auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); - - auto f0 = - std::make_shared>(X(1), X(2), between, model0); - auto f1 = - std::make_shared>(X(1), X(2), between, model1); - std::vector factors{f0, f1}; - - // Create via toFactorGraph - using symbol_shorthand::Z; - Matrix H0_1, H0_2, H1_1, H1_2; - Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2); - std::vector> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/}, - // - {X(1), H0_1 /*Sp1*/}, - {X(2), H0_2 /*Tp2*/}}; - - Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2); - std::vector> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/}, - // - {X(1), H1_1 /*Sp1*/}, - {X(2), H1_2 /*Tp2*/}}; - auto gm = new gtsam::GaussianMixture( - {Z(1)}, {X(1), X(2)}, {m1}, - {std::make_shared(terms0, 1, -d0, model0), - std::make_shared(terms1, 1, -d1, model1)}); - gtsam::HybridBayesNet bn; - bn.emplace_back(gm); - - gtsam::VectorValues measurements; - measurements.insert(Z(1), gtsam::Z_1x1); - // Create FG with single GaussianMixtureFactor - HybridGaussianFactorGraph mixture_fg = bn.toFactorGraph(measurements); - - // Linearized prior factor on X1 - auto prior = PriorFactor(X(1), x1, prior_noise).linearize(values); - mixture_fg.push_back(prior); - - auto hbn = mixture_fg.eliminateSequential(); - - VectorValues cv; - cv.insert(X(1), Vector1(0.0)); - cv.insert(X(2), Vector1(0.0)); - - // Check that we get different error values at the MLE point μ. - AlgebraicDecisionTree errorTree = hbn->errorTree(cv); - - HybridValues hv0(cv, DiscreteValues{{M(1), 0}}); - HybridValues hv1(cv, DiscreteValues{{M(1), 1}}); - - auto cond0 = hbn->at(0)->asMixture(); - auto cond1 = hbn->at(1)->asMixture(); - auto discrete_cond = hbn->at(2)->asDiscrete(); - AlgebraicDecisionTree expectedErrorTree(m1, 9.90348755254, - 0.69314718056); - EXPECT(assert_equal(expectedErrorTree, errorTree)); -} - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 861e19cc9..b04878ac5 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -243,25 +243,5 @@ namespace gtsam { } /* ************************************************************************* */ - double GaussianBayesNet::logNormalizationConstant() const { - /* - normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma)) - logConstant = -0.5 * n*log(2*pi) - 0.5 * log det(Sigma) - - log det(Sigma)) = -2.0 * logDeterminant() - thus, logConstant = -0.5*n*log(2*pi) + logDeterminant() - - BayesNet logConstant = sum(-0.5*n_i*log(2*pi) + logDeterminant_i()) - = sum(-0.5*n_i*log(2*pi)) + sum(logDeterminant_i()) - = sum(-0.5*n_i*log(2*pi)) + bn->logDeterminant() - */ - double logNormConst = 0.0; - for (const sharedConditional& cg : *this) { - logNormConst += cg->logNormalizationConstant(); - } - return logNormConst; - } - - /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index ea1cb8603..19781d1e6 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -82,12 +82,6 @@ namespace gtsam { /** Check equality */ bool equals(const This& bn, double tol = 1e-9) const; - /// Check exact equality. - friend bool operator==(const GaussianBayesNet& lhs, - const GaussianBayesNet& rhs) { - return lhs.isEqual(rhs); - } - /// print graph void print( const std::string& s = "", @@ -234,14 +228,6 @@ namespace gtsam { * @return The determinant */ double logDeterminant() const; - /** - * @brief Get the log of the normalization constant corresponding to the - * joint Gaussian density represented by this Bayes net. - * - * @return double - */ - double logNormalizationConstant() const; - /** * Backsubstitute with a different RHS vector than the one stored in this BayesNet. * gy=inv(R*inv(Sigma))*gx diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index f986eed02..0112835aa 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -121,7 +121,6 @@ namespace gtsam { const auto mean = solve({}); // solve for mean. mean.print(" mean", formatter); } - cout << " logNormalizationConstant: " << logNormalizationConstant() << std::endl; if (model_) model_->print(" Noise model: "); else @@ -185,13 +184,8 @@ namespace gtsam { double GaussianConditional::logNormalizationConstant() const { constexpr double log2pi = 1.8378770664093454835606594728112; size_t n = d().size(); - // Sigma = (R'R)^{-1}, det(Sigma) = det((R'R)^{-1}) = det(R'R)^{-1} - // log det(Sigma) = -log(det(R'R)) = -2*log(det(R)) - // Hence, log det(Sigma)) = -2.0 * logDeterminant() - // which gives log = -0.5*n*log(2*pi) - 0.5*(-2.0 * logDeterminant()) - // = -0.5*n*log(2*pi) + (0.5*2.0 * logDeterminant()) - // = -0.5*n*log(2*pi) + logDeterminant() - return -0.5 * n * log2pi + logDeterminant(); + // log det(Sigma)) = - 2.0 * logDeterminant() + return - 0.5 * n * log2pi + logDeterminant(); } /* ************************************************************************* */ diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 99ee4eb83..7fbd43ffc 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -263,11 +263,6 @@ namespace gtsam { /** equals required by Testable for unit testing */ bool equals(const VectorValues& x, double tol = 1e-9) const; - /// Check exact equality. - friend bool operator==(const VectorValues& lhs, const VectorValues& rhs) { - return lhs.equals(rhs); - } - /// @{ /// @name Advanced Interface /// @{ diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 3a629f349..4b0963b8d 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -510,17 +510,12 @@ virtual class GaussianConditional : gtsam::JacobianFactor { GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R, size_t name1, gtsam::Matrix S, size_t name2, gtsam::Matrix T, const gtsam::noiseModel::Diagonal* sigmas); - GaussianConditional(const vector> terms, - size_t nrFrontals, gtsam::Vector d, - const gtsam::noiseModel::Diagonal* sigmas); // Constructors with no noise model GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R); GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R, size_t name1, gtsam::Matrix S); GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R, size_t name1, gtsam::Matrix S, size_t name2, gtsam::Matrix T); - GaussianConditional(const gtsam::KeyVector& keys, size_t nrFrontals, - const gtsam::VerticalBlockMatrix& augmentedMatrix); // Named constructors static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key, diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index dcd821889..a4a722012 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -516,7 +516,6 @@ TEST(GaussianConditional, Print) { " d = [ 20 40 ]\n" " mean: 1 elements\n" " x0: 20 40\n" - " logNormalizationConstant: -4.0351\n" "isotropic dim=2 sigma=3\n"; EXPECT(assert_print_equal(expected, conditional, "GaussianConditional")); @@ -531,7 +530,6 @@ TEST(GaussianConditional, Print) { " S[x1] = [ -1 -2 ]\n" " [ -3 -4 ]\n" " d = [ 20 40 ]\n" - " logNormalizationConstant: -4.0351\n" "isotropic dim=2 sigma=3\n"; EXPECT(assert_print_equal(expected1, conditional1, "GaussianConditional")); @@ -547,7 +545,6 @@ TEST(GaussianConditional, Print) { " S[y1] = [ -5 -6 ]\n" " [ -7 -8 ]\n" " d = [ 20 40 ]\n" - " logNormalizationConstant: -4.0351\n" "isotropic dim=2 sigma=3\n"; EXPECT(assert_print_equal(expected2, conditional2, "GaussianConditional")); } From cea84b8c8945249f57905b3162d9e7cd7369d83f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 20 Aug 2024 15:50:39 -0400 Subject: [PATCH 48/71] reduce the diff even more --- gtsam/hybrid/GaussianMixture.cpp | 2 -- gtsam/hybrid/HybridBayesNet.cpp | 1 - gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp | 3 --- 3 files changed, 6 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 531a9f2e5..c105a329e 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -145,8 +145,6 @@ void GaussianMixture::print(const std::string &s, std::cout << "(" << formatter(dk.first) << ", " << dk.second << "), "; } std::cout << "\n"; - std::cout << " logNormalizationConstant: " << logConstant_ << "\n" - << std::endl; conditionals_.print( "", [&](Key k) { return formatter(k); }, [&](const GaussianConditional::shared_ptr &gf) -> std::string { diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index bb0f23686..b02967555 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -221,7 +221,6 @@ GaussianBayesNet HybridBayesNet::choose( HybridValues HybridBayesNet::optimize() const { // Collect all the discrete factors to compute MPE DiscreteBayesNet discrete_bn; - for (auto &&conditional : *this) { if (conditional->isDiscrete()) { discrete_bn.push_back(conditional->asDiscrete()); diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index cf56e35d6..93081d309 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -675,7 +675,6 @@ factor 6: P( m1 | m0 ): size: 3 conditional 0: Hybrid P( x0 | x1 m0) Discrete Keys = (m0, 2), - Choice(m0) 0 Leaf p(x0 | x1) R = [ 10.0499 ] @@ -691,7 +690,6 @@ conditional 0: Hybrid P( x0 | x1 m0) conditional 1: Hybrid P( x1 | x2 m0 m1) Discrete Keys = (m0, 2), (m1, 2), - Choice(m1) 0 Choice(m0) 0 0 Leaf p(x1 | x2) @@ -721,7 +719,6 @@ conditional 1: Hybrid P( x1 | x2 m0 m1) conditional 2: Hybrid P( x2 | m0 m1) Discrete Keys = (m0, 2), (m1, 2), - Choice(m1) 0 Choice(m0) 0 0 Leaf p(x2) From 7269d80b1c1077921944754487783a29e8fc354e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 20 Aug 2024 15:57:36 -0400 Subject: [PATCH 49/71] improvements to GaussianConditional and GaussianBayesNet --- gtsam/linear/GaussianBayesNet.cpp | 20 +++++++++++++++++++ gtsam/linear/GaussianBayesNet.h | 14 +++++++++++++ gtsam/linear/GaussianConditional.cpp | 10 ++++++++-- gtsam/linear/VectorValues.h | 5 +++++ gtsam/linear/linear.i | 5 +++++ .../linear/tests/testGaussianConditional.cpp | 3 +++ 6 files changed, 55 insertions(+), 2 deletions(-) diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index b04878ac5..861e19cc9 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -243,5 +243,25 @@ namespace gtsam { } /* ************************************************************************* */ + double GaussianBayesNet::logNormalizationConstant() const { + /* + normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma)) + logConstant = -0.5 * n*log(2*pi) - 0.5 * log det(Sigma) + + log det(Sigma)) = -2.0 * logDeterminant() + thus, logConstant = -0.5*n*log(2*pi) + logDeterminant() + + BayesNet logConstant = sum(-0.5*n_i*log(2*pi) + logDeterminant_i()) + = sum(-0.5*n_i*log(2*pi)) + sum(logDeterminant_i()) + = sum(-0.5*n_i*log(2*pi)) + bn->logDeterminant() + */ + double logNormConst = 0.0; + for (const sharedConditional& cg : *this) { + logNormConst += cg->logNormalizationConstant(); + } + return logNormConst; + } + + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 19781d1e6..ea1cb8603 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -82,6 +82,12 @@ namespace gtsam { /** Check equality */ bool equals(const This& bn, double tol = 1e-9) const; + /// Check exact equality. + friend bool operator==(const GaussianBayesNet& lhs, + const GaussianBayesNet& rhs) { + return lhs.isEqual(rhs); + } + /// print graph void print( const std::string& s = "", @@ -228,6 +234,14 @@ namespace gtsam { * @return The determinant */ double logDeterminant() const; + /** + * @brief Get the log of the normalization constant corresponding to the + * joint Gaussian density represented by this Bayes net. + * + * @return double + */ + double logNormalizationConstant() const; + /** * Backsubstitute with a different RHS vector than the one stored in this BayesNet. * gy=inv(R*inv(Sigma))*gx diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 0112835aa..f986eed02 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -121,6 +121,7 @@ namespace gtsam { const auto mean = solve({}); // solve for mean. mean.print(" mean", formatter); } + cout << " logNormalizationConstant: " << logNormalizationConstant() << std::endl; if (model_) model_->print(" Noise model: "); else @@ -184,8 +185,13 @@ namespace gtsam { double GaussianConditional::logNormalizationConstant() const { constexpr double log2pi = 1.8378770664093454835606594728112; size_t n = d().size(); - // log det(Sigma)) = - 2.0 * logDeterminant() - return - 0.5 * n * log2pi + logDeterminant(); + // Sigma = (R'R)^{-1}, det(Sigma) = det((R'R)^{-1}) = det(R'R)^{-1} + // log det(Sigma) = -log(det(R'R)) = -2*log(det(R)) + // Hence, log det(Sigma)) = -2.0 * logDeterminant() + // which gives log = -0.5*n*log(2*pi) - 0.5*(-2.0 * logDeterminant()) + // = -0.5*n*log(2*pi) + (0.5*2.0 * logDeterminant()) + // = -0.5*n*log(2*pi) + logDeterminant() + return -0.5 * n * log2pi + logDeterminant(); } /* ************************************************************************* */ diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 7fbd43ffc..99ee4eb83 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -263,6 +263,11 @@ namespace gtsam { /** equals required by Testable for unit testing */ bool equals(const VectorValues& x, double tol = 1e-9) const; + /// Check exact equality. + friend bool operator==(const VectorValues& lhs, const VectorValues& rhs) { + return lhs.equals(rhs); + } + /// @{ /// @name Advanced Interface /// @{ diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 4b0963b8d..3a629f349 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -510,12 +510,17 @@ virtual class GaussianConditional : gtsam::JacobianFactor { GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R, size_t name1, gtsam::Matrix S, size_t name2, gtsam::Matrix T, const gtsam::noiseModel::Diagonal* sigmas); + GaussianConditional(const vector> terms, + size_t nrFrontals, gtsam::Vector d, + const gtsam::noiseModel::Diagonal* sigmas); // Constructors with no noise model GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R); GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R, size_t name1, gtsam::Matrix S); GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R, size_t name1, gtsam::Matrix S, size_t name2, gtsam::Matrix T); + GaussianConditional(const gtsam::KeyVector& keys, size_t nrFrontals, + const gtsam::VerticalBlockMatrix& augmentedMatrix); // Named constructors static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key, diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index a4a722012..dcd821889 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -516,6 +516,7 @@ TEST(GaussianConditional, Print) { " d = [ 20 40 ]\n" " mean: 1 elements\n" " x0: 20 40\n" + " logNormalizationConstant: -4.0351\n" "isotropic dim=2 sigma=3\n"; EXPECT(assert_print_equal(expected, conditional, "GaussianConditional")); @@ -530,6 +531,7 @@ TEST(GaussianConditional, Print) { " S[x1] = [ -1 -2 ]\n" " [ -3 -4 ]\n" " d = [ 20 40 ]\n" + " logNormalizationConstant: -4.0351\n" "isotropic dim=2 sigma=3\n"; EXPECT(assert_print_equal(expected1, conditional1, "GaussianConditional")); @@ -545,6 +547,7 @@ TEST(GaussianConditional, Print) { " S[y1] = [ -5 -6 ]\n" " [ -7 -8 ]\n" " d = [ 20 40 ]\n" + " logNormalizationConstant: -4.0351\n" "isotropic dim=2 sigma=3\n"; EXPECT(assert_print_equal(expected2, conditional2, "GaussianConditional")); } From 8d54c4abe0fea13334fb2f6927639e53b250fae9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 20 Aug 2024 16:40:13 -0400 Subject: [PATCH 50/71] update VectorValues::== docstring --- gtsam/linear/VectorValues.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 99ee4eb83..2fa50b7f6 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -263,7 +263,7 @@ namespace gtsam { /** equals required by Testable for unit testing */ bool equals(const VectorValues& x, double tol = 1e-9) const; - /// Check exact equality. + /// Check equality. friend bool operator==(const VectorValues& lhs, const VectorValues& rhs) { return lhs.equals(rhs); } From a78ffe19e81d6ae41b385535046d1bd5cbfcf938 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 20 Aug 2024 16:45:19 -0400 Subject: [PATCH 51/71] update unit test to also check for GaussianBayesNet::logNormalizationConstant --- gtsam/linear/tests/testGaussianBayesNet.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index 966b70915..99453ee4e 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -80,6 +80,8 @@ TEST(GaussianBayesNet, Evaluate1) { smallBayesNet.at(0)->logNormalizationConstant() + smallBayesNet.at(1)->logNormalizationConstant(), 1e-9); + EXPECT_DOUBLES_EQUAL(log(constant), smallBayesNet.logNormalizationConstant(), + 1e-9); const double actual = smallBayesNet.evaluate(mean); EXPECT_DOUBLES_EQUAL(constant, actual, 1e-9); } From 910300b810cb32dcabf02e6286918f2442de6fec Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 20 Aug 2024 17:43:48 -0400 Subject: [PATCH 52/71] fix test --- gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 93081d309..751e84d91 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -680,12 +680,14 @@ conditional 0: Hybrid P( x0 | x1 m0) R = [ 10.0499 ] S[x1] = [ -0.0995037 ] d = [ -9.85087 ] + logNormalizationConstant: 1.38862 No noise model 1 Leaf p(x0 | x1) R = [ 10.0499 ] S[x1] = [ -0.0995037 ] d = [ -9.95037 ] + logNormalizationConstant: 1.38862 No noise model conditional 1: Hybrid P( x1 | x2 m0 m1) @@ -696,12 +698,14 @@ conditional 1: Hybrid P( x1 | x2 m0 m1) R = [ 10.099 ] S[x2] = [ -0.0990196 ] d = [ -9.99901 ] + logNormalizationConstant: 1.3935 No noise model 0 1 Leaf p(x1 | x2) R = [ 10.099 ] S[x2] = [ -0.0990196 ] d = [ -9.90098 ] + logNormalizationConstant: 1.3935 No noise model 1 Choice(m0) @@ -709,12 +713,14 @@ conditional 1: Hybrid P( x1 | x2 m0 m1) R = [ 10.099 ] S[x2] = [ -0.0990196 ] d = [ -10.098 ] + logNormalizationConstant: 1.3935 No noise model 1 1 Leaf p(x1 | x2) R = [ 10.099 ] S[x2] = [ -0.0990196 ] d = [ -10 ] + logNormalizationConstant: 1.3935 No noise model conditional 2: Hybrid P( x2 | m0 m1) @@ -726,6 +732,7 @@ conditional 2: Hybrid P( x2 | m0 m1) d = [ -10.1489 ] mean: 1 elements x2: -1.0099 + logNormalizationConstant: 1.38857 No noise model 0 1 Leaf p(x2) @@ -733,6 +740,7 @@ conditional 2: Hybrid P( x2 | m0 m1) d = [ -10.1479 ] mean: 1 elements x2: -1.0098 + logNormalizationConstant: 1.38857 No noise model 1 Choice(m0) @@ -741,6 +749,7 @@ conditional 2: Hybrid P( x2 | m0 m1) d = [ -10.0504 ] mean: 1 elements x2: -1.0001 + logNormalizationConstant: 1.38857 No noise model 1 1 Leaf p(x2) @@ -748,6 +757,7 @@ conditional 2: Hybrid P( x2 | m0 m1) d = [ -10.0494 ] mean: 1 elements x2: -1 + logNormalizationConstant: 1.38857 No noise model )"; From 73d971a3c6aef8985e947d824ba96ba2ebaae0a6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 21 Aug 2024 04:12:21 -0400 Subject: [PATCH 53/71] unit tests for AlgebraicDecisionTree helper methods --- .../tests/testAlgebraicDecisionTree.cpp | 49 +++++++++++++++++++ 1 file changed, 49 insertions(+) diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp index 19f4686c2..d65a9c82b 100644 --- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp +++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp @@ -593,6 +593,55 @@ TEST(ADT, zero) { EXPECT_DOUBLES_EQUAL(0, anotb(x11), 1e-9); } +/// Example ADT from 0 to 11. +ADT exampleADT() { + DiscreteKey A(0, 2), B(1, 3), C(2, 2); + ADT f(A & B & C, "0 6 2 8 4 10 1 7 3 9 5 11"); + return f; +} +/* ************************************************************************** */ +// Test sum +TEST(ADT, Sum) { + ADT a = exampleADT(); + double expected_sum = 0; + for (double i = 0; i < 12; i++) { + expected_sum += i; + } + EXPECT_DOUBLES_EQUAL(expected_sum, a.sum(), 1e-9); +} + +/* ************************************************************************** */ +// Test normalize +TEST(ADT, Normalize) { + ADT a = exampleADT(); + double sum = a.sum(); + auto actual = a.normalize(sum); + + DiscreteKey A(0, 2), B(1, 3), C(2, 2); + DiscreteKeys keys = DiscreteKeys{A, B, C}; + std::vector cpt{0 / sum, 6 / sum, 2 / sum, 8 / sum, + 4 / sum, 10 / sum, 1 / sum, 7 / sum, + 3 / sum, 9 / sum, 5 / sum, 11 / sum}; + ADT expected(keys, cpt); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************** */ +// Test min +TEST(ADT, Min) { + ADT a = exampleADT(); + double min = a.min(); + EXPECT_DOUBLES_EQUAL(0.0, min, 1e-9); +} + +/* ************************************************************************** */ +// Test max +TEST(ADT, Max) { + ADT a = exampleADT(); + double max = a.max(); + EXPECT_DOUBLES_EQUAL(11.0, max, 1e-9); +} + /* ************************************************************************* */ int main() { TestResult tr; From 7765314638432c267eaf1631c61786c3b55f79d6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Aug 2024 05:23:52 -0400 Subject: [PATCH 54/71] fix return type of EliminateQR --- gtsam/linear/linear.i | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 3a629f349..ed19eaa3b 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -375,7 +375,8 @@ virtual class JacobianFactor : gtsam::GaussianFactor { void serialize() const; }; -pair EliminateQR(const gtsam::GaussianFactorGraph& factors, const gtsam::Ordering& keys); +pair EliminateQR( + const gtsam::GaussianFactorGraph& factors, const gtsam::Ordering& keys); #include virtual class HessianFactor : gtsam::GaussianFactor { From bf70087fff9d5454a311039375b3c8512d2d47d7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 04:35:02 -0400 Subject: [PATCH 55/71] Make default PartialPriorFactor constructor public --- gtsam_unstable/slam/PartialPriorFactor.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index b70fe8b19..7722e5d82 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -50,9 +50,6 @@ namespace gtsam { Vector prior_; ///< Measurement on tangent space parameters, in compressed form. std::vector indices_; ///< Indices of the measured tangent space parameters. - /** default constructor - only use for serialization */ - PartialPriorFactor() {} - /** * constructor with just minimum requirements for a factor - allows more * computation in the constructor. This should only be used by subclasses @@ -65,7 +62,8 @@ namespace gtsam { // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; - ~PartialPriorFactor() override {} + /** default constructor - only use for serialization */ + PartialPriorFactor() {} /** Single Element Constructor: Prior on a single parameter at index 'idx' in the tangent vector.*/ PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) : @@ -85,6 +83,8 @@ namespace gtsam { assert(model->dim() == (size_t)prior.size()); } + ~PartialPriorFactor() override {} + /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { return std::static_pointer_cast( From 2e39fef721f8e1dd9a824d6c0e3fed55080e7bd8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 04:35:16 -0400 Subject: [PATCH 56/71] wrap PartialPriorFactor --- gtsam_unstable/gtsam_unstable.i | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index 8566934dd..3beb845fb 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -671,6 +671,21 @@ class AHRS { //void print(string s) const; }; +#include +template +virtual class PartialPriorFactor : gtsam::NoiseModelFactor { + PartialPriorFactor(gtsam::Key key, size_t idx, double prior, + const gtsam::noiseModel::Base* model); + PartialPriorFactor(gtsam::Key key, const std::vector& indices, + const gtsam::Vector& prior, + const gtsam::noiseModel::Base* model); + + // enabling serialization functionality + void serialize() const; + + const gtsam::Vector& prior(); +}; + // Tectonic SAM Factors #include From 95da15a61abac9b6bf66e25e0193841964d86870 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 04:35:33 -0400 Subject: [PATCH 57/71] allow stl binding for pybind11 in gtsam_unstable --- python/CMakeLists.txt | 2 ++ python/gtsam_unstable/gtsam_unstable.tpl | 1 + 2 files changed, 3 insertions(+) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index ba55ac2af..0c9dd6967 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -189,6 +189,8 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::BinaryMeasurementsPoint3 gtsam::BinaryMeasurementsUnit3 gtsam::BinaryMeasurementsRot3 + gtsam::SimWall2DVector + gtsam::SimPolygon2DVector gtsam::CameraSetCal3_S2 gtsam::CameraSetCal3Bundler gtsam::CameraSetCal3Unified diff --git a/python/gtsam_unstable/gtsam_unstable.tpl b/python/gtsam_unstable/gtsam_unstable.tpl index b98872c46..006ca7fc8 100644 --- a/python/gtsam_unstable/gtsam_unstable.tpl +++ b/python/gtsam_unstable/gtsam_unstable.tpl @@ -9,6 +9,7 @@ #include #include +#include #include #include #include From b94ab31e1f058d3fac9596922a6bda01fb1423a5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 04:36:00 -0400 Subject: [PATCH 58/71] localization example script which gives exact same result as C++ version --- .../examples/LocalizationExample.py | 68 +++++++++++++++++++ 1 file changed, 68 insertions(+) create mode 100644 python/gtsam_unstable/examples/LocalizationExample.py diff --git a/python/gtsam_unstable/examples/LocalizationExample.py b/python/gtsam_unstable/examples/LocalizationExample.py new file mode 100644 index 000000000..ad8266d6d --- /dev/null +++ b/python/gtsam_unstable/examples/LocalizationExample.py @@ -0,0 +1,68 @@ +""" +A simple 2D pose slam example with "GPS" measurements + - The robot moves forward 2 meter each iteration + - The robot initially faces along the X axis (horizontal, to the right in 2D) + - We have full odometry between pose + - We have "GPS-like" measurements implemented with a custom factor +""" +import numpy as np + +import gtsam +from gtsam import BetweenFactorPose2, Pose2, noiseModel +from gtsam_unstable import PartialPriorFactorPose2 + + +def main(): + # 1. Create a factor graph container and add factors to it. + graph = gtsam.NonlinearFactorGraph() + + # 2a. Add odometry factors + # For simplicity, we will use the same noise model for each odometry factor + odometryNoise = noiseModel.Diagonal.Sigmas(np.asarray([0.2, 0.2, 0.1])) + + # Create odometry (Between) factors between consecutive poses + graph.push_back( + BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise)) + graph.push_back( + BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise)) + + # 2b. Add "GPS-like" measurements + # We will use PartialPrior factor for this. + unaryNoise = noiseModel.Diagonal.Sigmas(np.array([0.1, + 0.1])) # 10cm std on x,y + + graph.push_back( + PartialPriorFactorPose2(1, [0, 1], np.asarray([0.0, 0.0]), unaryNoise)) + graph.push_back( + PartialPriorFactorPose2(2, [0, 1], np.asarray([2.0, 0.0]), unaryNoise)) + graph.push_back( + PartialPriorFactorPose2(3, [0, 1], np.asarray([4.0, 0.0]), unaryNoise)) + graph.print("\nFactor Graph:\n") + + # 3. Create the data structure to hold the initialEstimate estimate to the solution + # For illustrative purposes, these have been deliberately set to incorrect values + initialEstimate = gtsam.Values() + initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)) + initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2)) + initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1)) + initialEstimate.print("\nInitial Estimate:\n") + + # 4. Optimize using Levenberg-Marquardt optimization. The optimizer + # accepts an optional set of configuration parameters, controlling + # things like convergence criteria, the type of linear system solver + # to use, and the amount of information displayed during optimization. + # Here we will use the default set of parameters. See the + # documentation for the full set of parameters. + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) + result = optimizer.optimize() + result.print("Final Result:\n") + + # 5. Calculate and print marginal covariances for all variables + marginals = gtsam.Marginals(graph, result) + print("x1 covariance:\n", marginals.marginalCovariance(1)) + print("x2 covariance:\n", marginals.marginalCovariance(2)) + print("x3 covariance:\n", marginals.marginalCovariance(3)) + + +if __name__ == "__main__": + main() From c57d5a0f7052b9a3b4c34b95b1d795bebe312632 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 07:23:31 -0400 Subject: [PATCH 59/71] Updated porting progress --- python/gtsam/examples/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/examples/README.md b/python/gtsam/examples/README.md index 2a2c9f2ef..70f6cfcb0 100644 --- a/python/gtsam/examples/README.md +++ b/python/gtsam/examples/README.md @@ -17,7 +17,7 @@ | InverseKinematicsExampleExpressions.cpp | | | ISAM2Example_SmartFactor | | | ISAM2_SmartFactorStereo_IMU | | -| LocalizationExample | impossible? | +| LocalizationExample | :heavy_check_mark: | | METISOrderingExample | | | OdometryExample | :heavy_check_mark: | | PlanarSLAMExample | :heavy_check_mark: | From 353ff644fcdf51ac3c9e94f99b9e2574dd1fe686 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 24 Jul 2022 12:40:11 -0400 Subject: [PATCH 60/71] generate python type hints during pip build --- python/setup.py.in | 1 + 1 file changed, 1 insertion(+) diff --git a/python/setup.py.in b/python/setup.py.in index 824a6656e..b9d7392c7 100644 --- a/python/setup.py.in +++ b/python/setup.py.in @@ -13,6 +13,7 @@ package_data = { "./*.so", "./*.dll", "./*.pyd", + "*.pyi", "**/*.pyi", # Add the type hints ] } From 51aadbda3411411597d7f0d53bede212e898dc08 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 24 Jul 2022 22:25:56 -0400 Subject: [PATCH 61/71] add type hints and make command to test gtsam_unstable for python --- python/CMakeLists.txt | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index ba55ac2af..2068ee8dc 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -266,7 +266,9 @@ endif() # Add custom target so we can install with `make python-install` set(GTSAM_PYTHON_INSTALL_TARGET python-install) add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} - COMMAND ${PYTHON_EXECUTABLE} -m pip install --user . + COMMAND stubgen -q -p gtsam && + rsync -a "out/gtsam/" gtsam && + ${PYTHON_EXECUTABLE} -m pip install --user . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) @@ -279,3 +281,12 @@ add_custom_target( ${PYTHON_EXECUTABLE} -m unittest discover -v -s . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} ${GTSAM_PYTHON_TEST_FILES} WORKING_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/tests") + +add_custom_target( + python-test-unstable + COMMAND + ${CMAKE_COMMAND} -E env # add package to python path so no need to install + "PYTHONPATH=${GTSAM_PYTHON_BUILD_DIRECTORY}/$ENV{PYTHONPATH}" + ${PYTHON_EXECUTABLE} -m unittest discover -v -s . + DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} ${GTSAM_PYTHON_TEST_FILES} + WORKING_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable/tests") From 48215b9063045921bba4268db36e3ac8085687e3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 14:03:43 -0400 Subject: [PATCH 62/71] remove duplicate target --- python/CMakeLists.txt | 9 --------- 1 file changed, 9 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 2068ee8dc..20f2eab4b 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -281,12 +281,3 @@ add_custom_target( ${PYTHON_EXECUTABLE} -m unittest discover -v -s . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} ${GTSAM_PYTHON_TEST_FILES} WORKING_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/tests") - -add_custom_target( - python-test-unstable - COMMAND - ${CMAKE_COMMAND} -E env # add package to python path so no need to install - "PYTHONPATH=${GTSAM_PYTHON_BUILD_DIRECTORY}/$ENV{PYTHONPATH}" - ${PYTHON_EXECUTABLE} -m unittest discover -v -s . - DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} ${GTSAM_PYTHON_TEST_FILES} - WORKING_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable/tests") From f2d69ed697b047e8fa5c89cc4613ffcee89c3c88 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 14:47:14 -0400 Subject: [PATCH 63/71] always use bundled pybind11 inside wrap --- python/CMakeLists.txt | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index ba55ac2af..12d8a9b4d 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -29,11 +29,8 @@ if(POLICY CMP0057) cmake_policy(SET CMP0057 NEW) endif() -# Prefer system pybind11 first, if not found, rely on bundled version: -find_package(pybind11 CONFIG QUIET) -if (NOT pybind11_FOUND) - add_subdirectory(${PROJECT_SOURCE_DIR}/wrap/pybind11 pybind11) -endif() +# Use bundled pybind11 version +find_package(pybind11 CONFIG QUIET PATHS "${PROJECT_SOURCE_DIR}/wrap/pybind11") # Set the wrapping script variable set(PYBIND_WRAP_SCRIPT "${PROJECT_SOURCE_DIR}/wrap/scripts/pybind_wrap.py") From e58a5c4cac810db86c3f1a960e61db81446126b4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 15:41:55 -0400 Subject: [PATCH 64/71] directly add pybind11 subdirectory so files are generated correctly --- python/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 12d8a9b4d..b8c2c718f 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -30,7 +30,7 @@ if(POLICY CMP0057) endif() # Use bundled pybind11 version -find_package(pybind11 CONFIG QUIET PATHS "${PROJECT_SOURCE_DIR}/wrap/pybind11") +add_subdirectory(${PROJECT_SOURCE_DIR}/wrap/pybind11 pybind11) # Set the wrapping script variable set(PYBIND_WRAP_SCRIPT "${PROJECT_SOURCE_DIR}/wrap/scripts/pybind_wrap.py") From bde9285211b1882865a59f870eb3a8f1bb6d2395 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 16:42:23 -0400 Subject: [PATCH 65/71] better test documentation --- tests/testDoglegOptimizer.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index d7de63d3d..6fbc522d3 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -184,6 +184,14 @@ TEST(DoglegOptimizer, Constraint) { } /* ************************************************************************* */ +/** + * Test created to fix issue in ISAM2 when using the DogLegOptimizer. + * Originally reported by kvmanohar22 in issue #301 + * https://github.com/borglab/gtsam/issues/301 + * + * This test is based on a script provided by kvmanohar22 + * to help reproduce the issue. + */ TEST(DogLegOptimizer, VariableUpdate) { // Make the typename short so it looks much cleaner typedef SmartProjectionPoseFactor SmartFactor; From 5f43b69c4371ca8d784378d11256f25412874a81 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 16:45:23 -0400 Subject: [PATCH 66/71] add mypy to the dev_requirements so we can get stubgen --- python/dev_requirements.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/python/dev_requirements.txt b/python/dev_requirements.txt index 6970ee613..c76d3bf1b 100644 --- a/python/dev_requirements.txt +++ b/python/dev_requirements.txt @@ -1,2 +1,3 @@ -r requirements.txt -pyparsing>=2.4.2 \ No newline at end of file +pyparsing>=2.4.2 +mypy>=1.11.2 \ No newline at end of file From 0e73367345f50f9c3a0a7f6e3beb7be124d99f23 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 18:57:03 -0400 Subject: [PATCH 67/71] stubgen only for Windows until we can find the rsync equivalent --- python/CMakeLists.txt | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 20f2eab4b..b32d51c68 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -265,12 +265,18 @@ endif() # Add custom target so we can install with `make python-install` set(GTSAM_PYTHON_INSTALL_TARGET python-install) -add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} - COMMAND stubgen -q -p gtsam && - rsync -a "out/gtsam/" gtsam && - ${PYTHON_EXECUTABLE} -m pip install --user . +if (NOT WIN32) + add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} + COMMAND stubgen -q -p gtsam && rsync -a \"out/gtsam/\" gtsam && ${PYTHON_EXECUTABLE} -m pip install --user . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) +elseif() + #TODO(Varun) Find rsync equivalent on Windows + add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} + COMMAND ${PYTHON_EXECUTABLE} -m pip install --user . + DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} + WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) +endif() # Custom make command to run all GTSAM Python tests add_custom_target( From bfcd5eb08a9feea9f84b079878396afae63e5b54 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 26 Aug 2024 05:48:18 -0400 Subject: [PATCH 68/71] use older version of mypy --- python/dev_requirements.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/dev_requirements.txt b/python/dev_requirements.txt index c76d3bf1b..6a5e7f924 100644 --- a/python/dev_requirements.txt +++ b/python/dev_requirements.txt @@ -1,3 +1,3 @@ -r requirements.txt pyparsing>=2.4.2 -mypy>=1.11.2 \ No newline at end of file +mypy==1.4.1 #TODO(Varun) A bug in mypy>=1.5.0 causes an unresolved placeholder error when importing numpy>=2.0.0 (https://github.com/python/mypy/issues/17396) \ No newline at end of file From da99cf0e192ca4033d34575714b7591f343258ff Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 26 Aug 2024 06:25:17 -0400 Subject: [PATCH 69/71] remove extraneous variable --- python/CMakeLists.txt | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index b32d51c68..677ee72ce 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -264,15 +264,14 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) endif() # Add custom target so we can install with `make python-install` -set(GTSAM_PYTHON_INSTALL_TARGET python-install) if (NOT WIN32) - add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} + add_custom_target(python-install COMMAND stubgen -q -p gtsam && rsync -a \"out/gtsam/\" gtsam && ${PYTHON_EXECUTABLE} -m pip install --user . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) elseif() #TODO(Varun) Find rsync equivalent on Windows - add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} + add_custom_target(python-install COMMAND ${PYTHON_EXECUTABLE} -m pip install --user . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) From 48f9d0b11664b5e69e4f8cd925851e3206ecb5af Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 26 Aug 2024 09:44:28 -0400 Subject: [PATCH 70/71] fix if-else mistype --- python/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 677ee72ce..1e4acf142 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -264,12 +264,12 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) endif() # Add custom target so we can install with `make python-install` -if (NOT WIN32) +if (NOT WIN32) # WIN32=1 is target platform is Windows add_custom_target(python-install COMMAND stubgen -q -p gtsam && rsync -a \"out/gtsam/\" gtsam && ${PYTHON_EXECUTABLE} -m pip install --user . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) -elseif() +else() #TODO(Varun) Find rsync equivalent on Windows add_custom_target(python-install COMMAND ${PYTHON_EXECUTABLE} -m pip install --user . From 1744c4aeb3b8875b8c17651905eebb4c7687d4ef Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 26 Aug 2024 10:07:24 -0400 Subject: [PATCH 71/71] save stubs to 'stubs' directory and use cp instead of rsync --- python/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 1e4acf142..bdab178ba 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -266,11 +266,11 @@ endif() # Add custom target so we can install with `make python-install` if (NOT WIN32) # WIN32=1 is target platform is Windows add_custom_target(python-install - COMMAND stubgen -q -p gtsam && rsync -a \"out/gtsam/\" gtsam && ${PYTHON_EXECUTABLE} -m pip install --user . + COMMAND stubgen -q -p gtsam -o ./stubs && cp -a stubs/gtsam/ gtsam && ${PYTHON_EXECUTABLE} -m pip install --user . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) else() - #TODO(Varun) Find rsync equivalent on Windows + #TODO(Varun) Find equivalent cp on Windows add_custom_target(python-install COMMAND ${PYTHON_EXECUTABLE} -m pip install --user . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES}