From 8f5689b7624ed5ae090439de7abf29fce4cf5b89 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 2 Jan 2023 09:44:03 -0500 Subject: [PATCH 1/5] minor readjustment --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 10e12bf77..442ffb428 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -238,13 +238,14 @@ hybridElimination(const HybridGaussianFactorGraph &factors, keysOfEliminated = conditional_factor.first->keys(); keysOfSeparator = conditional_factor.second->keys(); + GaussianConditional::shared_ptr conditional = conditional_factor.first; + // Get the log of the log normalization constant inverse. + double logZ = -conditional->logNormalizationConstant() + graph_z.constant; + #ifdef HYBRID_TIMING gttoc_(hybrid_eliminate); #endif - GaussianConditional::shared_ptr conditional = conditional_factor.first; - // Get the log of the log normalization constant inverse. - double logZ = -conditional->logNormalizationConstant() + graph_z.constant; return {conditional, {conditional_factor.second, logZ}}; }; From cae98e1d3e276722e29d5047360adfd3985371c5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 2 Jan 2023 09:44:43 -0500 Subject: [PATCH 2/5] rename eliminate to eliminateFunc --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 442ffb428..133011b6a 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -218,7 +218,8 @@ hybridElimination(const HybridGaussianFactorGraph &factors, KeyVector keysOfSeparator; // This is the elimination method on the leaf nodes - auto eliminate = [&](const GaussianMixtureFactor::GraphAndConstant &graph_z) + auto eliminateFunc = + [&](const GaussianMixtureFactor::GraphAndConstant &graph_z) -> EliminationPair { if (graph_z.graph.empty()) { return {nullptr, {nullptr, 0.0}}; @@ -250,7 +251,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, }; // Perform elimination! - DecisionTree eliminationResults(sum, eliminate); + DecisionTree eliminationResults(sum, eliminateFunc); #ifdef HYBRID_TIMING tictoc_print_(); From e3a63aa77c902d49eb5fe033d6fc1034476d2dd4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 2 Jan 2023 10:03:56 -0500 Subject: [PATCH 3/5] check for 0 sum --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 133011b6a..b68c9286b 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -285,6 +285,10 @@ hybridElimination(const HybridGaussianFactorGraph &factors, double sum = 0.0; auto visitor = [&](double y) { sum += y; }; fdt.visit(visitor); + // Check if sum is 0, and update accordingly. + if (sum == 0) { + sum = 1.0; + } // fdt = DecisionTree(fdt, // [sum](const double &x) { return x / sum; // }); From 40a67b517cb8151cde633b38ec3cacfb8c08506d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 2 Jan 2023 10:04:50 -0500 Subject: [PATCH 4/5] prune nonlinear by calling method rather than on the bayestree --- gtsam/hybrid/tests/testHybridNonlinearISAM.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index db0dc73c3..8b5bb41ac 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -357,10 +357,9 @@ TEST(HybridNonlinearISAM, Incremental_approximate) { // Run update with pruning size_t maxComponents = 5; incrementalHybrid.update(graph1, initial); + incrementalHybrid.prune(maxComponents); HybridGaussianISAM bayesTree = incrementalHybrid.bayesTree(); - bayesTree.prune(maxComponents); - // Check if we have a bayes tree with 4 hybrid nodes, // each with 2, 4, 8, and 5 (pruned) leaves respetively. EXPECT_LONGS_EQUAL(4, bayesTree.size()); @@ -382,10 +381,9 @@ TEST(HybridNonlinearISAM, Incremental_approximate) { // Run update with pruning a second time. incrementalHybrid.update(graph2, initial); + incrementalHybrid.prune(maxComponents); bayesTree = incrementalHybrid.bayesTree(); - bayesTree.prune(maxComponents); - // Check if we have a bayes tree with pruned hybrid nodes, // with 5 (pruned) leaves. CHECK_EQUAL(5, bayesTree.size()); From 5c87d7d51beaa0f163eedc15edfab13274ab4b3c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 3 Jan 2023 00:50:16 -0500 Subject: [PATCH 5/5] disable test instead of commenting out --- gtsam/hybrid/tests/testHybridEstimation.cpp | 26 ++++++++++----------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 6d222e322..ad51ccfe1 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -114,7 +114,7 @@ TEST(HybridEstimation, Full) { /****************************************************************************/ // Test approximate inference with an additional pruning step. -TEST(HybridEstimation, Incremental) { +TEST_DISABLED(HybridEstimation, Incremental) { size_t K = 15; std::vector measurements = {0, 1, 2, 2, 2, 2, 3, 4, 5, 6, 6, 7, 8, 9, 9, 9, 10, 11, 11, 11, 11}; @@ -154,21 +154,21 @@ TEST(HybridEstimation, Incremental) { /*TODO(Varun) Gives degenerate result due to probability underflow. Need to normalize probabilities. */ - // HybridValues delta = smoother.hybridBayesNet().optimize(); + HybridValues delta = smoother.hybridBayesNet().optimize(); - // Values result = initial.retract(delta.continuous()); + Values result = initial.retract(delta.continuous()); - // DiscreteValues expected_discrete; - // for (size_t k = 0; k < K - 1; k++) { - // expected_discrete[M(k)] = discrete_seq[k]; - // } - // EXPECT(assert_equal(expected_discrete, delta.discrete())); + DiscreteValues expected_discrete; + for (size_t k = 0; k < K - 1; k++) { + expected_discrete[M(k)] = discrete_seq[k]; + } + EXPECT(assert_equal(expected_discrete, delta.discrete())); - // Values expected_continuous; - // for (size_t k = 0; k < K; k++) { - // expected_continuous.insert(X(k), measurements[k]); - // } - // EXPECT(assert_equal(expected_continuous, result)); + Values expected_continuous; + for (size_t k = 0; k < K; k++) { + expected_continuous.insert(X(k), measurements[k]); + } + EXPECT(assert_equal(expected_continuous, result)); } /**