From 8a268d6e13d78e62388e39275db194fce77b47ef Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 27 Oct 2024 17:16:20 -0400 Subject: [PATCH 01/21] improve HybridGaussianISAM test --- gtsam/hybrid/tests/testHybridGaussianISAM.cpp | 22 ++++++++++--------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 11e3194f2..ce5e72dad 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -259,27 +259,27 @@ TEST(HybridGaussianElimination, Approx_inference) { /* ****************************************************************************/ // Test approximate inference with an additional pruning step. -TEST(HybridGaussianElimination, IncrementalApproximate) { +TEST_DISABLED(HybridGaussianElimination, IncrementalApproximate) { Switching switching(5); HybridGaussianISAM incrementalHybrid; - HybridGaussianFactorGraph graph1; + HybridGaussianFactorGraph graph; /***** Run Round 1 *****/ // Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3 for (size_t i = 1; i < 4; i++) { - graph1.push_back(switching.linearizedFactorGraph.at(i)); + graph.push_back(switching.linearizedFactorGraph.at(i)); } // Add the Gaussian factors, 1 prior on X(0), + graph.push_back(switching.linearizedFactorGraph.at(0)); // 3 measurements on X(1), X(2), X(3) - graph1.push_back(switching.linearizedFactorGraph.at(0)); for (size_t i = 5; i <= 7; i++) { - graph1.push_back(switching.linearizedFactorGraph.at(i)); + graph.push_back(switching.linearizedFactorGraph.at(i)); } // Run update with pruning size_t maxComponents = 5; - incrementalHybrid.update(graph1); + incrementalHybrid.update(graph); incrementalHybrid.prune(maxComponents); // Check if we have a bayes tree with 4 hybrid nodes, @@ -295,12 +295,14 @@ TEST(HybridGaussianElimination, IncrementalApproximate) { 5, incrementalHybrid[X(3)]->conditional()->asHybrid()->nrComponents()); /***** Run Round 2 *****/ - HybridGaussianFactorGraph graph2; - graph2.push_back(switching.linearizedFactorGraph.at(4)); - graph2.push_back(switching.linearizedFactorGraph.at(8)); + graph.resize(0); // clear all factors + // Add hybrid factor X(3)-X(4) + graph.push_back(switching.linearizedFactorGraph.at(4)); + // Add measurement factor X(4) + graph.push_back(switching.linearizedFactorGraph.at(8)); // Run update with pruning a second time. - incrementalHybrid.update(graph2); + incrementalHybrid.update(graph); incrementalHybrid.prune(maxComponents); // Check if we have a bayes tree with pruned hybrid nodes, From 9db13c5987dcd115832282ee5d2975132038cba6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 28 Oct 2024 18:48:47 -0400 Subject: [PATCH 02/21] better test naming --- gtsam/hybrid/tests/testHybridNonlinearISAM.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index c5f52e3eb..d6cff24fd 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -196,7 +196,7 @@ TEST(HybridNonlinearISAM, IncrementalInference) { /* ****************************************************************************/ // Test if we can approximately do the inference -TEST(HybridNonlinearISAM, Approx_inference) { +TEST(HybridNonlinearISAM, ApproxInference) { Switching switching(4); HybridNonlinearISAM incrementalHybrid; HybridNonlinearFactorGraph graph1; @@ -304,7 +304,7 @@ TEST(HybridNonlinearISAM, Approx_inference) { /* ****************************************************************************/ // Test approximate inference with an additional pruning step. -TEST(HybridNonlinearISAM, Incremental_approximate) { +TEST(HybridNonlinearISAM, IncrementalApproximate) { Switching switching(5); HybridNonlinearISAM incrementalHybrid; HybridNonlinearFactorGraph graph1; From 1f68a3d9b184fa76429dd98f75d56e6a00866a10 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 28 Oct 2024 18:48:57 -0400 Subject: [PATCH 03/21] fix test --- gtsam/hybrid/tests/testHybridNonlinearISAM.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index d6cff24fd..192f43742 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -83,7 +83,7 @@ TEST(HybridNonlinearISAM, IncrementalElimination) { HybridNonlinearFactorGraph graph2; initial = Values(); - graph1.push_back(switching.unaryFactors.at(1)); // P(X1) + graph2.push_back(switching.unaryFactors.at(1)); // P(X1) graph2.push_back(switching.unaryFactors.at(2)); // P(X2) graph2.push_back(switching.modeChain.at(1)); // P(M0, M1) From a7e48c0a81214cba7793630b687a22bc8e2878d0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 28 Oct 2024 18:50:39 -0400 Subject: [PATCH 04/21] use single graph in tests --- .../hybrid/tests/testHybridNonlinearISAM.cpp | 68 +++++++++---------- 1 file changed, 34 insertions(+), 34 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index 192f43742..94747ae79 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -49,7 +49,7 @@ using symbol_shorthand::Z; TEST(HybridNonlinearISAM, IncrementalElimination) { Switching switching(3); HybridNonlinearISAM isam; - HybridNonlinearFactorGraph graph1; + HybridNonlinearFactorGraph graph; Values initial; // Create initial factor graph @@ -57,17 +57,17 @@ TEST(HybridNonlinearISAM, IncrementalElimination) { // | | | // X0 -*- X1 -*- X2 // \*-M0-*/ - graph1.push_back(switching.unaryFactors.at(0)); // P(X0) - graph1.push_back(switching.binaryFactors.at(0)); // P(X0, X1 | M0) - graph1.push_back(switching.binaryFactors.at(1)); // P(X1, X2 | M1) - graph1.push_back(switching.modeChain.at(0)); // P(M0) + graph.push_back(switching.unaryFactors.at(0)); // P(X0) + graph.push_back(switching.binaryFactors.at(0)); // P(X0, X1 | M0) + graph.push_back(switching.binaryFactors.at(1)); // P(X1, X2 | M1) + graph.push_back(switching.modeChain.at(0)); // P(M0) initial.insert(X(0), 1); initial.insert(X(1), 2); initial.insert(X(2), 3); // Run update step - isam.update(graph1, initial); + isam.update(graph, initial); // Check that after update we have 3 hybrid Bayes net nodes: // P(X0 | X1, M0) and P(X1, X2 | M0, M1), P(M0, M1) @@ -80,14 +80,14 @@ TEST(HybridNonlinearISAM, IncrementalElimination) { /********************************************************/ // New factor graph for incremental update. - HybridNonlinearFactorGraph graph2; + graph = HybridNonlinearFactorGraph(); initial = Values(); - graph2.push_back(switching.unaryFactors.at(1)); // P(X1) - graph2.push_back(switching.unaryFactors.at(2)); // P(X2) - graph2.push_back(switching.modeChain.at(1)); // P(M0, M1) + graph.push_back(switching.unaryFactors.at(1)); // P(X1) + graph.push_back(switching.unaryFactors.at(2)); // P(X2) + graph.push_back(switching.modeChain.at(1)); // P(M0, M1) - isam.update(graph2, initial); + isam.update(graph, initial); bayesTree = isam.bayesTree(); // Check that after the second update we have @@ -103,7 +103,7 @@ TEST(HybridNonlinearISAM, IncrementalElimination) { TEST(HybridNonlinearISAM, IncrementalInference) { Switching switching(3); HybridNonlinearISAM isam; - HybridNonlinearFactorGraph graph1; + HybridNonlinearFactorGraph graph; Values initial; // Create initial factor graph @@ -112,16 +112,16 @@ TEST(HybridNonlinearISAM, IncrementalInference) { // X0 -*- X1 -*- X2 // | | // *-M0 - * - M1 - graph1.push_back(switching.unaryFactors.at(0)); // P(X0) - graph1.push_back(switching.binaryFactors.at(0)); // P(X0, X1 | M0) - graph1.push_back(switching.unaryFactors.at(1)); // P(X1) - graph1.push_back(switching.modeChain.at(0)); // P(M0) + graph.push_back(switching.unaryFactors.at(0)); // P(X0) + graph.push_back(switching.binaryFactors.at(0)); // P(X0, X1 | M0) + graph.push_back(switching.unaryFactors.at(1)); // P(X1) + graph.push_back(switching.modeChain.at(0)); // P(M0) initial.insert(X(0), 1); initial.insert(X(1), 2); // Run update step - isam.update(graph1, initial); + isam.update(graph, initial); HybridGaussianISAM bayesTree = isam.bayesTree(); auto discreteConditional_m0 = bayesTree[M(0)]->conditional()->asDiscrete(); @@ -129,16 +129,16 @@ TEST(HybridNonlinearISAM, IncrementalInference) { /********************************************************/ // New factor graph for incremental update. - HybridNonlinearFactorGraph graph2; + graph = HybridNonlinearFactorGraph(); initial = Values(); initial.insert(X(2), 3); - graph2.push_back(switching.binaryFactors.at(1)); // P(X1, X2 | M1) - graph2.push_back(switching.unaryFactors.at(2)); // P(X2) - graph2.push_back(switching.modeChain.at(1)); // P(M0, M1) + graph.push_back(switching.binaryFactors.at(1)); // P(X1, X2 | M1) + graph.push_back(switching.unaryFactors.at(2)); // P(X2) + graph.push_back(switching.modeChain.at(1)); // P(M0, M1) - isam.update(graph2, initial); + isam.update(graph, initial); bayesTree = isam.bayesTree(); /********************************************************/ @@ -199,18 +199,18 @@ TEST(HybridNonlinearISAM, IncrementalInference) { TEST(HybridNonlinearISAM, ApproxInference) { Switching switching(4); HybridNonlinearISAM incrementalHybrid; - HybridNonlinearFactorGraph graph1; + HybridNonlinearFactorGraph graph; Values initial; // Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3 for (size_t i = 0; i < 3; i++) { - graph1.push_back(switching.binaryFactors.at(i)); + graph.push_back(switching.binaryFactors.at(i)); } // Add the Gaussian factors, 1 prior on X(0), // 3 measurements on X(1), X(2), X(3) for (size_t i = 0; i < 4; i++) { - graph1.push_back(switching.unaryFactors.at(i)); + graph.push_back(switching.unaryFactors.at(i)); initial.insert(X(i), i + 1); } @@ -228,7 +228,7 @@ TEST(HybridNonlinearISAM, ApproxInference) { .BaseEliminateable::eliminatePartialMultifrontal(ordering); size_t maxNrLeaves = 5; - incrementalHybrid.update(graph1, initial); + incrementalHybrid.update(graph, initial); HybridGaussianISAM bayesTree = incrementalHybrid.bayesTree(); bayesTree.prune(maxNrLeaves); @@ -307,19 +307,19 @@ TEST(HybridNonlinearISAM, ApproxInference) { TEST(HybridNonlinearISAM, IncrementalApproximate) { Switching switching(5); HybridNonlinearISAM incrementalHybrid; - HybridNonlinearFactorGraph graph1; + HybridNonlinearFactorGraph graph; Values initial; /***** Run Round 1 *****/ // Add the 3 hybrid factors, x0-x1, x1-x2, x2-x3 for (size_t i = 0; i < 3; i++) { - graph1.push_back(switching.binaryFactors.at(i)); + graph.push_back(switching.binaryFactors.at(i)); } // Add the Gaussian factors, 1 prior on X(0), // 3 measurements on X(1), X(2), X(3) for (size_t i = 0; i < 4; i++) { - graph1.push_back(switching.unaryFactors.at(i)); + graph.push_back(switching.unaryFactors.at(i)); initial.insert(X(i), i + 1); } @@ -328,7 +328,7 @@ TEST(HybridNonlinearISAM, IncrementalApproximate) { // Run update with pruning size_t maxComponents = 5; - incrementalHybrid.update(graph1, initial); + incrementalHybrid.update(graph, initial); incrementalHybrid.prune(maxComponents); HybridGaussianISAM bayesTree = incrementalHybrid.bayesTree(); @@ -345,14 +345,14 @@ TEST(HybridNonlinearISAM, IncrementalApproximate) { 5, bayesTree[X(3)]->conditional()->asHybrid()->nrComponents()); /***** Run Round 2 *****/ - HybridGaussianFactorGraph graph2; - graph2.push_back(switching.binaryFactors.at(3)); // x3-x4 - graph2.push_back(switching.unaryFactors.at(4)); // x4 measurement + graph = HybridGaussianFactorGraph(); + graph.push_back(switching.binaryFactors.at(3)); // x3-x4 + graph.push_back(switching.unaryFactors.at(4)); // x4 measurement initial = Values(); initial.insert(X(4), 5); // Run update with pruning a second time. - incrementalHybrid.update(graph2, initial); + incrementalHybrid.update(graph, initial); incrementalHybrid.prune(maxComponents); bayesTree = incrementalHybrid.bayesTree(); From 7c672bb91b1ad28acace110fffb4293e923d524d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 28 Oct 2024 18:52:43 -0400 Subject: [PATCH 05/21] minor improvements --- gtsam/hybrid/tests/testHybridNonlinearISAM.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index 94747ae79..e42adc4ec 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -195,7 +195,7 @@ TEST(HybridNonlinearISAM, IncrementalInference) { } /* ****************************************************************************/ -// Test if we can approximately do the inference +// Test if we can approximately do the inference (using pruning) TEST(HybridNonlinearISAM, ApproxInference) { Switching switching(4); HybridNonlinearISAM incrementalHybrid; @@ -325,7 +325,6 @@ TEST(HybridNonlinearISAM, IncrementalApproximate) { // TODO(Frank): no mode chain? - // Run update with pruning size_t maxComponents = 5; incrementalHybrid.update(graph, initial); @@ -347,7 +346,7 @@ TEST(HybridNonlinearISAM, IncrementalApproximate) { /***** Run Round 2 *****/ graph = HybridGaussianFactorGraph(); graph.push_back(switching.binaryFactors.at(3)); // x3-x4 - graph.push_back(switching.unaryFactors.at(4)); // x4 measurement + graph.push_back(switching.unaryFactors.at(4)); // x4 measurement initial = Values(); initial.insert(X(4), 5); From 3c50f9387c4b84d3e1627de5e179e680c29279f2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Oct 2024 01:55:04 -0400 Subject: [PATCH 06/21] add pruning support for HybridNonlinearFactor --- gtsam/hybrid/HybridNonlinearFactor.cpp | 31 ++++++++++++++++++++++++++ gtsam/hybrid/HybridNonlinearFactor.h | 7 ++++++ 2 files changed, 38 insertions(+) diff --git a/gtsam/hybrid/HybridNonlinearFactor.cpp b/gtsam/hybrid/HybridNonlinearFactor.cpp index 6ffb95511..48c327156 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.cpp +++ b/gtsam/hybrid/HybridNonlinearFactor.cpp @@ -16,6 +16,7 @@ * @date Sep 12, 2024 */ +#include #include #include #include @@ -202,4 +203,34 @@ std::shared_ptr HybridNonlinearFactor::linearize( linearized_factors); } +/* *******************************************************************************/ +HybridNonlinearFactor::shared_ptr HybridNonlinearFactor::prune( + const DecisionTreeFactor& discreteProbs) const { + // Find keys in discreteProbs.keys() but not in this->keys(): + std::set mine(this->keys().begin(), this->keys().end()); + std::set theirs(discreteProbs.keys().begin(), + discreteProbs.keys().end()); + std::vector diff; + std::set_difference(theirs.begin(), theirs.end(), mine.begin(), mine.end(), + std::back_inserter(diff)); + + // Find maximum probability value for every combination of our keys. + Ordering keys(diff); + auto max = discreteProbs.max(keys); + + // Check the max value for every combination of our keys. + // If the max value is 0.0, we can prune the corresponding conditional. + auto pruner = + [&](const Assignment& choices, + const NonlinearFactorValuePair& pair) -> NonlinearFactorValuePair { + if (max->evaluate(choices) == 0.0) + return {nullptr, std::numeric_limits::infinity()}; + else + return pair; + }; + + FactorValuePairs prunedFactors = factors().apply(pruner); + return std::make_shared(discreteKeys(), prunedFactors); +} + } // namespace gtsam \ No newline at end of file diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h index 325fa3eaa..e264b1d10 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.h +++ b/gtsam/hybrid/HybridNonlinearFactor.h @@ -166,6 +166,9 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor { /// @} + /// Getter for NonlinearFactor decision tree + const FactorValuePairs& factors() const { return factors_; } + /// Linearize specific nonlinear factors based on the assignment in /// discreteValues. GaussianFactor::shared_ptr linearize( @@ -176,6 +179,10 @@ class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor { std::shared_ptr linearize( const Values& continuousValues) const; + /// Prune this factor based on the discrete probabilities. + HybridNonlinearFactor::shared_ptr prune( + const DecisionTreeFactor& discreteProbs) const; + private: /// Helper struct to assist private constructor below. struct ConstructorHelper; From 3c06f0755139ec93e4b473b988c35fea8bc1adec Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Oct 2024 01:56:03 -0400 Subject: [PATCH 07/21] check if NonlinearFactor is valid before linearizing --- gtsam/hybrid/HybridNonlinearFactor.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam/hybrid/HybridNonlinearFactor.cpp b/gtsam/hybrid/HybridNonlinearFactor.cpp index 48c327156..6f52df917 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.cpp +++ b/gtsam/hybrid/HybridNonlinearFactor.cpp @@ -185,6 +185,11 @@ std::shared_ptr HybridNonlinearFactor::linearize( [continuousValues]( const std::pair& f) -> GaussianFactorValuePair { auto [factor, val] = f; + // Check if valid factor. If not, return null and infinite error. + if (!factor) { + return {nullptr, std::numeric_limits::infinity()}; + } + if (auto gaussian = std::dynamic_pointer_cast( factor->noiseModel())) { return {factor->linearize(continuousValues), From b4c7d3af81bf13f71ed19858b88869a592e7b8e3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Oct 2024 01:56:58 -0400 Subject: [PATCH 08/21] formatting --- gtsam/hybrid/HybridGaussianFactor.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index 6c06c1c0a..6d1064647 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -148,6 +148,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { /// Getter for GaussianFactor decision tree const FactorValuePairs &factors() const { return factors_; } + /** * @brief Helper function to return factors and functional to create a * DecisionTree of Gaussian Factor Graphs. From 649da80c91ef68b2de2571e0fe14485dc652365c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Oct 2024 10:03:23 -0400 Subject: [PATCH 09/21] prune nonlinear factors in HybridNonlinearISAM --- gtsam/hybrid/HybridNonlinearISAM.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridNonlinearISAM.cpp b/gtsam/hybrid/HybridNonlinearISAM.cpp index 503afaa72..37a556096 100644 --- a/gtsam/hybrid/HybridNonlinearISAM.cpp +++ b/gtsam/hybrid/HybridNonlinearISAM.cpp @@ -16,6 +16,7 @@ */ #include +#include #include #include @@ -39,7 +40,6 @@ void HybridNonlinearISAM::update(const HybridNonlinearFactorGraph& newFactors, if (newFactors.size() > 0) { // Reorder and relinearize every reorderInterval updates if (reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) { - // TODO(Varun) Re-linearization doesn't take into account pruning reorderRelinearize(); reorderCounter_ = 0; } @@ -65,8 +65,22 @@ void HybridNonlinearISAM::reorderRelinearize() { // Obtain the new linearization point const Values newLinPoint = estimate(); + auto discreteProbs = *(isam_.roots().at(0)->conditional()->asDiscrete()); + isam_.clear(); + // Prune nonlinear factors based on discrete conditional probabilities + HybridNonlinearFactorGraph pruned_factors; + for (auto&& factor : factors_) { + if (auto nf = std::dynamic_pointer_cast(factor)) { + pruned_factors.push_back(nf->prune(discreteProbs)); + } else { + pruned_factors.push_back(factor); + } + } + factors_ = pruned_factors; + factors_.print("OG factors"); + // Just recreate the whole BayesTree // TODO: allow for constrained ordering here // TODO: decouple re-linearization and reordering to avoid From 4d5f1c0f43523a0074f17f18cb9ff93860410da8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Oct 2024 10:37:17 -0400 Subject: [PATCH 10/21] formatting --- gtsam/hybrid/HybridNonlinearFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridNonlinearFactor.cpp b/gtsam/hybrid/HybridNonlinearFactor.cpp index 6f52df917..376bc66f1 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.cpp +++ b/gtsam/hybrid/HybridNonlinearFactor.cpp @@ -238,4 +238,4 @@ HybridNonlinearFactor::shared_ptr HybridNonlinearFactor::prune( return std::make_shared(discreteKeys(), prunedFactors); } -} // namespace gtsam \ No newline at end of file +} // namespace gtsam From 095a4cd1876ba9085e9d1cb74ab1eb8854721972 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Oct 2024 13:09:27 -0400 Subject: [PATCH 11/21] remove print --- gtsam/hybrid/HybridNonlinearISAM.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/hybrid/HybridNonlinearISAM.cpp b/gtsam/hybrid/HybridNonlinearISAM.cpp index 37a556096..29e467d86 100644 --- a/gtsam/hybrid/HybridNonlinearISAM.cpp +++ b/gtsam/hybrid/HybridNonlinearISAM.cpp @@ -79,7 +79,6 @@ void HybridNonlinearISAM::reorderRelinearize() { } } factors_ = pruned_factors; - factors_.print("OG factors"); // Just recreate the whole BayesTree // TODO: allow for constrained ordering here From ae95c6e84aed4377df50367f3e1859418fa7bcb3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Oct 2024 13:50:10 -0400 Subject: [PATCH 12/21] clean up tests and TODOs --- gtsam/hybrid/tests/testHybridBayesTree.cpp | 25 +++++----------------- 1 file changed, 5 insertions(+), 20 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridBayesTree.cpp b/gtsam/hybrid/tests/testHybridBayesTree.cpp index 6da991173..db298e6fc 100644 --- a/gtsam/hybrid/tests/testHybridBayesTree.cpp +++ b/gtsam/hybrid/tests/testHybridBayesTree.cpp @@ -74,9 +74,7 @@ TEST(HybridGaussianFactorGraph, hfg.add(HybridGaussianFactor(m1, two::components(X(1)))); hfg.add(DecisionTreeFactor(m1, {2, 8})); - // TODO(Varun) Adding extra discrete variable not connected to continuous - // variable throws segfault - // hfg.add(DecisionTreeFactor({m1, m2, "1 2 3 4")); + hfg.add(DecisionTreeFactor({m1, m2}, "1 2 3 4")); HybridBayesTree::shared_ptr result = hfg.eliminateMultifrontal(); @@ -176,7 +174,7 @@ TEST(HybridGaussianFactorGraph, Switching) { // Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7), // X(5), // M(1), M(4), M(2), M(6), M(8), M(3), M(7), M(5)}); - KeyVector ordering; + Ordering ordering; { std::vector naturalX(N); @@ -187,10 +185,6 @@ TEST(HybridGaussianFactorGraph, Switching) { auto [ndX, lvls] = makeBinaryOrdering(ordX); std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering)); - // TODO(dellaert): this has no effect! - for (auto& l : lvls) { - l = -l; - } } { std::vector naturalC(N - 1); @@ -199,14 +193,11 @@ TEST(HybridGaussianFactorGraph, Switching) { std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC), [](int x) { return M(x); }); - // std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering)); const auto [ndC, lvls] = makeBinaryOrdering(ordC); std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering)); } - auto ordering_full = Ordering(ordering); - const auto [hbt, remaining] = - hfg->eliminatePartialMultifrontal(ordering_full); + const auto [hbt, remaining] = hfg->eliminatePartialMultifrontal(ordering); // 12 cliques in the bayes tree and 0 remaining variables to eliminate. EXPECT_LONGS_EQUAL(12, hbt->size()); @@ -230,7 +221,7 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) { // Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7), // X(5), // M(1), M(4), M(2), M(6), M(8), M(3), M(7), M(5)}); - KeyVector ordering; + Ordering ordering; { std::vector naturalX(N); @@ -241,10 +232,6 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) { auto [ndX, lvls] = makeBinaryOrdering(ordX); std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering)); - // TODO(dellaert): this has no effect! - for (auto& l : lvls) { - l = -l; - } } { std::vector naturalC(N - 1); @@ -257,10 +244,8 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) { const auto [ndC, lvls] = makeBinaryOrdering(ordC); std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering)); } - auto ordering_full = Ordering(ordering); - const auto [hbt, remaining] = - hfg->eliminatePartialMultifrontal(ordering_full); + const auto [hbt, remaining] = hfg->eliminatePartialMultifrontal(ordering); auto new_fg = makeSwitchingChain(12); auto isam = HybridGaussianISAM(*hbt); From dfc91469bceb135cbec6e0f038be45c89dcde576 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Oct 2024 14:45:19 -0400 Subject: [PATCH 13/21] discreteFactors method --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 11 +++++++++++ gtsam/hybrid/HybridGaussianFactorGraph.h | 8 ++++++++ gtsam/hybrid/tests/testHybridBayesTree.cpp | 7 +------ gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp | 9 ++------- 4 files changed, 22 insertions(+), 13 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index ceabe0871..049e6c38d 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -580,4 +580,15 @@ GaussianFactorGraph HybridGaussianFactorGraph::choose( return gfg; } +/* ************************************************************************ */ +DiscreteFactorGraph HybridGaussianFactorGraph::discreteFactors() const { + DiscreteFactorGraph dfg; + for (auto &&f : factors_) { + auto discreteFactor = std::dynamic_pointer_cast(f); + assert(discreteFactor); + dfg.push_back(discreteFactor); + } + return dfg; +} + } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index 048fd2701..c2e50ace8 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -254,6 +254,14 @@ class GTSAM_EXPORT HybridGaussianFactorGraph GaussianFactorGraph operator()(const DiscreteValues& assignment) const { return choose(assignment); } + + /** + * @brief Helper method to get all the discrete factors + * as a DiscreteFactorGraph. + * + * @return DiscreteFactorGraph + */ + DiscreteFactorGraph discreteFactors() const; }; // traits diff --git a/gtsam/hybrid/tests/testHybridBayesTree.cpp b/gtsam/hybrid/tests/testHybridBayesTree.cpp index db298e6fc..4f5583bf5 100644 --- a/gtsam/hybrid/tests/testHybridBayesTree.cpp +++ b/gtsam/hybrid/tests/testHybridBayesTree.cpp @@ -443,12 +443,7 @@ TEST(HybridBayesTree, Optimize) { const auto [hybridBayesNet, remainingFactorGraph] = s.linearizedFactorGraph.eliminatePartialSequential(ordering); - DiscreteFactorGraph dfg; - for (auto&& f : *remainingFactorGraph) { - auto discreteFactor = dynamic_pointer_cast(f); - assert(discreteFactor); - dfg.push_back(discreteFactor); - } + DiscreteFactorGraph dfg = remainingFactorGraph->discreteFactors(); // Add the probabilities for each branch DiscreteKeys discrete_keys = {{M(0), 2}, {M(1), 2}, {M(2), 2}}; diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index dd4128034..100eb024a 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -479,13 +479,8 @@ TEST(HybridNonlinearFactorGraph, Full_Elimination) { const auto [hybridBayesNet_partial, remainingFactorGraph_partial] = linearizedFactorGraph.eliminatePartialSequential(ordering); - DiscreteFactorGraph discrete_fg; - // TODO(Varun) Make this a function of HybridGaussianFactorGraph? - for (auto &factor : (*remainingFactorGraph_partial)) { - auto df = dynamic_pointer_cast(factor); - assert(df); - discrete_fg.push_back(df); - } + DiscreteFactorGraph discrete_fg = + remainingFactorGraph_partial->discreteFactors(); ordering.clear(); for (size_t k = 0; k < self.K - 1; k++) ordering.push_back(M(k)); From 6734cd332ff540d8cfb92a044fe9985decf56aee Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 29 Oct 2024 14:45:43 -0400 Subject: [PATCH 14/21] formatting --- gtsam/hybrid/tests/Switching.h | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index fdf9092b6..270792701 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -52,7 +52,8 @@ using symbol_shorthand::X; * @return HybridGaussianFactorGraph::shared_ptr */ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain( - size_t K, std::function x = X, std::function m = M) { + size_t K, std::function x = X, std::function m = M, + const std::string &transitionProbabilityTable = "0 1 1 3") { HybridGaussianFactorGraph hfg; hfg.add(JacobianFactor(x(1), I_3x3, Z_3x1)); @@ -68,7 +69,8 @@ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain( hfg.add(HybridGaussianFactor({m(k), 2}, components)); if (k > 1) { - hfg.add(DecisionTreeFactor({{m(k - 1), 2}, {m(k), 2}}, "0 1 1 3")); + hfg.add(DecisionTreeFactor({{m(k - 1), 2}, {m(k), 2}}, + transitionProbabilityTable)); } } @@ -171,8 +173,8 @@ struct Switching { // Add "motion models" ϕ(X(k),X(k+1),M(k)). for (size_t k = 0; k < K - 1; k++) { auto motion_models = motionModels(k, between_sigma); - nonlinearFactorGraph_.emplace_shared(modes[k], - motion_models); + nonlinearFactorGraph_.emplace_shared( + modes[k], motion_models); binaryFactors.push_back(nonlinearFactorGraph_.back()); } @@ -193,7 +195,8 @@ struct Switching { linearizationPoint.insert(X(k), static_cast(k + 1)); } - linearizedFactorGraph = *nonlinearFactorGraph_.linearize(linearizationPoint); + linearizedFactorGraph = + *nonlinearFactorGraph_.linearize(linearizationPoint); } // Create motion models for a given time step From d985f2fc257650a3e149a53739ae1154cece352e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 30 Oct 2024 18:21:14 -0400 Subject: [PATCH 15/21] add missing header --- gtsam/hybrid/HybridGaussianFactorGraph.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index c2e50ace8..e3c1e2d55 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include #include From f5f878e6fa0c99326155a88a5e27820cb71f9ee1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 1 Nov 2024 14:30:31 -0400 Subject: [PATCH 16/21] update test group --- gtsam/hybrid/tests/testHybridMotionModel.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridMotionModel.cpp b/gtsam/hybrid/tests/testHybridMotionModel.cpp index 16a2bd476..867f22cc6 100644 --- a/gtsam/hybrid/tests/testHybridMotionModel.cpp +++ b/gtsam/hybrid/tests/testHybridMotionModel.cpp @@ -124,7 +124,7 @@ std::pair approximateDiscreteMarginal( * the posterior probability of m1 should be 0.5/0.5. * Getting a measurement on z1 gives use more information. */ -TEST(HybridGaussianFactor, TwoStateModel) { +TEST(HybridGaussianFactorGraph, TwoStateModel) { double mu0 = 1.0, mu1 = 3.0; double sigma = 0.5; auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma, sigma); @@ -178,7 +178,7 @@ TEST(HybridGaussianFactor, TwoStateModel) { * the P(m1) should be 0.5/0.5. * Getting a measurement on z1 gives use more information. */ -TEST(HybridGaussianFactor, TwoStateModel2) { +TEST(HybridGaussianFactorGraph, TwoStateModel2) { double mu0 = 1.0, mu1 = 3.0; double sigma0 = 0.5, sigma1 = 2.0; auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma0, sigma1); @@ -281,7 +281,7 @@ TEST(HybridGaussianFactor, TwoStateModel2) { * the p(m1) should be 0.5/0.5. * Getting a measurement on z1 gives use more information. */ -TEST(HybridGaussianFactor, TwoStateModel3) { +TEST(HybridGaussianFactorGraph, TwoStateModel3) { double mu = 1.0; double sigma0 = 0.5, sigma1 = 2.0; auto hybridMotionModel = CreateHybridMotionModel(mu, mu, sigma0, sigma1); @@ -366,7 +366,7 @@ TEST(HybridGaussianFactor, TwoStateModel3) { * measurements and vastly different motion model: either stand still or move * far. This yields a very informative posterior. */ -TEST(HybridGaussianFactor, TwoStateModel4) { +TEST(HybridGaussianFactorGraph, TwoStateModel4) { double mu0 = 0.0, mu1 = 10.0; double sigma0 = 0.2, sigma1 = 5.0; auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma0, sigma1); From 01829381da5ec0fca7a2b9c382fae75eb4327f84 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 1 Nov 2024 14:31:27 -0400 Subject: [PATCH 17/21] move direct FG motion model test to testHybridMotionModel.cpp --- .../hybrid/tests/testHybridGaussianFactor.cpp | 158 ----------------- gtsam/hybrid/tests/testHybridMotionModel.cpp | 162 +++++++++++++++++- 2 files changed, 160 insertions(+), 160 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp index 05e05c79b..9642796ab 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp @@ -194,164 +194,6 @@ TEST(HybridGaussianFactor, Error) { 4.0, hybridFactor.error({continuousValues, discreteValues}), 1e-9); } -/* ************************************************************************* */ -namespace test_direct_factor_graph { -/** - * @brief Create a Factor Graph by directly specifying all - * the factors instead of creating conditionals first. - * This way we can directly provide the likelihoods and - * then perform linearization. - * - * @param values Initial values to linearize around. - * @param means The means of the HybridGaussianFactor components. - * @param sigmas The covariances of the HybridGaussianFactor components. - * @param m1 The discrete key. - * @return HybridGaussianFactorGraph - */ -static HybridGaussianFactorGraph CreateFactorGraph( - const gtsam::Values &values, const std::vector &means, - const std::vector &sigmas, DiscreteKey &m1, - double measurement_noise = 1e-3) { - auto model0 = noiseModel::Isotropic::Sigma(1, sigmas[0]); - auto model1 = noiseModel::Isotropic::Sigma(1, sigmas[1]); - auto prior_noise = noiseModel::Isotropic::Sigma(1, measurement_noise); - - auto f0 = - std::make_shared>(X(0), X(1), means[0], model0) - ->linearize(values); - auto f1 = - std::make_shared>(X(0), X(1), means[1], model1) - ->linearize(values); - - // Create HybridGaussianFactor - // We take negative since we want - // the underlying scalar to be log(\sqrt(|2πΣ|)) - std::vector factors{{f0, model0->negLogConstant()}, - {f1, model1->negLogConstant()}}; - HybridGaussianFactor motionFactor(m1, factors); - - HybridGaussianFactorGraph hfg; - hfg.push_back(motionFactor); - - hfg.push_back(PriorFactor(X(0), values.at(X(0)), prior_noise) - .linearize(values)); - - return hfg; -} -} // namespace test_direct_factor_graph - -/* ************************************************************************* */ -/** - * @brief Test components with differing means but the same covariances. - * The factor graph is - * *-X1-*-X2 - * | - * M1 - */ -TEST(HybridGaussianFactor, DifferentMeansFG) { - using namespace test_direct_factor_graph; - - DiscreteKey m1(M(1), 2); - - Values values; - double x1 = 0.0, x2 = 1.75; - values.insert(X(0), x1); - values.insert(X(1), x2); - - std::vector means = {0.0, 2.0}, sigmas = {1e-0, 1e-0}; - - HybridGaussianFactorGraph hfg = CreateFactorGraph(values, means, sigmas, m1); - - { - auto bn = hfg.eliminateSequential(); - HybridValues actual = bn->optimize(); - - HybridValues expected( - VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(-1.75)}}, - DiscreteValues{{M(1), 0}}); - - EXPECT(assert_equal(expected, actual)); - - DiscreteValues dv0{{M(1), 0}}; - VectorValues cont0 = bn->optimize(dv0); - double error0 = bn->error(HybridValues(cont0, dv0)); - // regression - EXPECT_DOUBLES_EQUAL(0.69314718056, error0, 1e-9); - - DiscreteValues dv1{{M(1), 1}}; - VectorValues cont1 = bn->optimize(dv1); - double error1 = bn->error(HybridValues(cont1, dv1)); - EXPECT_DOUBLES_EQUAL(error0, error1, 1e-9); - } - - { - auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); - hfg.push_back( - PriorFactor(X(1), means[1], prior_noise).linearize(values)); - - auto bn = hfg.eliminateSequential(); - HybridValues actual = bn->optimize(); - - HybridValues expected( - VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(0.25)}}, - DiscreteValues{{M(1), 1}}); - - EXPECT(assert_equal(expected, actual)); - - { - DiscreteValues dv{{M(1), 0}}; - VectorValues cont = bn->optimize(dv); - double error = bn->error(HybridValues(cont, dv)); - // regression - EXPECT_DOUBLES_EQUAL(2.12692448787, error, 1e-9); - } - { - DiscreteValues dv{{M(1), 1}}; - VectorValues cont = bn->optimize(dv); - double error = bn->error(HybridValues(cont, dv)); - // regression - EXPECT_DOUBLES_EQUAL(0.126928487854, error, 1e-9); - } - } -} - -/* ************************************************************************* */ -/** - * @brief Test components with differing covariances but the same means. - * The factor graph is - * *-X1-*-X2 - * | - * M1 - */ -TEST(HybridGaussianFactor, DifferentCovariancesFG) { - using namespace test_direct_factor_graph; - - DiscreteKey m1(M(1), 2); - - Values values; - double x1 = 1.0, x2 = 1.0; - values.insert(X(0), x1); - values.insert(X(1), x2); - - std::vector means = {0.0, 0.0}, sigmas = {1e2, 1e-2}; - - // Create FG with HybridGaussianFactor and prior on X1 - HybridGaussianFactorGraph fg = CreateFactorGraph(values, means, sigmas, m1); - auto hbn = fg.eliminateSequential(); - - VectorValues cv; - cv.insert(X(0), Vector1(0.0)); - cv.insert(X(1), Vector1(0.0)); - - DiscreteValues dv0{{M(1), 0}}; - DiscreteValues dv1{{M(1), 1}}; - - DiscreteConditional expected_m1(m1, "0.5/0.5"); - DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete()); - - EXPECT(assert_equal(expected_m1, actual_m1)); -} - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/hybrid/tests/testHybridMotionModel.cpp b/gtsam/hybrid/tests/testHybridMotionModel.cpp index 867f22cc6..747a1b688 100644 --- a/gtsam/hybrid/tests/testHybridMotionModel.cpp +++ b/gtsam/hybrid/tests/testHybridMotionModel.cpp @@ -198,7 +198,7 @@ TEST(HybridGaussianFactorGraph, TwoStateModel2) { {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}}, VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) { vv.insert(given); // add measurements for HBN - const auto& expectedDiscretePosterior = hbn.discretePosterior(vv); + const auto &expectedDiscretePosterior = hbn.discretePosterior(vv); // Equality of posteriors asserts that the factor graph is correct (same // ratios for all modes) @@ -234,7 +234,7 @@ TEST(HybridGaussianFactorGraph, TwoStateModel2) { {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}}, VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) { vv.insert(given); // add measurements for HBN - const auto& expectedDiscretePosterior = hbn.discretePosterior(vv); + const auto &expectedDiscretePosterior = hbn.discretePosterior(vv); // Equality of posteriors asserts that the factor graph is correct (same // ratios for all modes) @@ -385,6 +385,164 @@ TEST(HybridGaussianFactorGraph, TwoStateModel4) { EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002)); } +/* ************************************************************************* */ +namespace test_direct_factor_graph { +/** + * @brief Create a Factor Graph by directly specifying all + * the factors instead of creating conditionals first. + * This way we can directly provide the likelihoods and + * then perform linearization. + * + * @param values Initial values to linearize around. + * @param means The means of the HybridGaussianFactor components. + * @param sigmas The covariances of the HybridGaussianFactor components. + * @param m1 The discrete key. + * @return HybridGaussianFactorGraph + */ +static HybridGaussianFactorGraph CreateFactorGraph( + const gtsam::Values &values, const std::vector &means, + const std::vector &sigmas, DiscreteKey &m1, + double measurement_noise = 1e-3) { + auto model0 = noiseModel::Isotropic::Sigma(1, sigmas[0]); + auto model1 = noiseModel::Isotropic::Sigma(1, sigmas[1]); + auto prior_noise = noiseModel::Isotropic::Sigma(1, measurement_noise); + + auto f0 = + std::make_shared>(X(0), X(1), means[0], model0) + ->linearize(values); + auto f1 = + std::make_shared>(X(0), X(1), means[1], model1) + ->linearize(values); + + // Create HybridGaussianFactor + // We take negative since we want + // the underlying scalar to be log(\sqrt(|2πΣ|)) + std::vector factors{{f0, model0->negLogConstant()}, + {f1, model1->negLogConstant()}}; + HybridGaussianFactor motionFactor(m1, factors); + + HybridGaussianFactorGraph hfg; + hfg.push_back(motionFactor); + + hfg.push_back(PriorFactor(X(0), values.at(X(0)), prior_noise) + .linearize(values)); + + return hfg; +} +} // namespace test_direct_factor_graph + +/* ************************************************************************* */ +/** + * @brief Test components with differing means but the same covariances. + * The factor graph is + * *-X1-*-X2 + * | + * M1 + */ +TEST(HybridGaussianFactorGraph, DifferentMeans) { + using namespace test_direct_factor_graph; + + DiscreteKey m1(M(1), 2); + + Values values; + double x1 = 0.0, x2 = 1.75; + values.insert(X(0), x1); + values.insert(X(1), x2); + + std::vector means = {0.0, 2.0}, sigmas = {1e-0, 1e-0}; + + HybridGaussianFactorGraph hfg = CreateFactorGraph(values, means, sigmas, m1); + + { + auto bn = hfg.eliminateSequential(); + HybridValues actual = bn->optimize(); + + HybridValues expected( + VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(-1.75)}}, + DiscreteValues{{M(1), 0}}); + + EXPECT(assert_equal(expected, actual)); + + DiscreteValues dv0{{M(1), 0}}; + VectorValues cont0 = bn->optimize(dv0); + double error0 = bn->error(HybridValues(cont0, dv0)); + // regression + EXPECT_DOUBLES_EQUAL(0.69314718056, error0, 1e-9); + + DiscreteValues dv1{{M(1), 1}}; + VectorValues cont1 = bn->optimize(dv1); + double error1 = bn->error(HybridValues(cont1, dv1)); + EXPECT_DOUBLES_EQUAL(error0, error1, 1e-9); + } + + { + auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); + hfg.push_back( + PriorFactor(X(1), means[1], prior_noise).linearize(values)); + + auto bn = hfg.eliminateSequential(); + HybridValues actual = bn->optimize(); + + HybridValues expected( + VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(0.25)}}, + DiscreteValues{{M(1), 1}}); + + EXPECT(assert_equal(expected, actual)); + + { + DiscreteValues dv{{M(1), 0}}; + VectorValues cont = bn->optimize(dv); + double error = bn->error(HybridValues(cont, dv)); + // regression + EXPECT_DOUBLES_EQUAL(2.12692448787, error, 1e-9); + } + { + DiscreteValues dv{{M(1), 1}}; + VectorValues cont = bn->optimize(dv); + double error = bn->error(HybridValues(cont, dv)); + // regression + EXPECT_DOUBLES_EQUAL(0.126928487854, error, 1e-9); + } + } +} + +/* ************************************************************************* */ +/** + * @brief Test components with differing covariances but the same means. + * The factor graph is + * *-X1-*-X2 + * | + * M1 + */ +TEST(HybridGaussianFactorGraph, DifferentCovariances) { + using namespace test_direct_factor_graph; + + DiscreteKey m1(M(1), 2); + + Values values; + double x1 = 1.0, x2 = 1.0; + values.insert(X(0), x1); + values.insert(X(1), x2); + + std::vector means = {0.0, 0.0}, sigmas = {1e2, 1e-2}; + + // Create FG with HybridGaussianFactor and prior on X1 + HybridGaussianFactorGraph fg = CreateFactorGraph(values, means, sigmas, m1); + auto hbn = fg.eliminateSequential(); + + VectorValues cv; + cv.insert(X(0), Vector1(0.0)); + cv.insert(X(1), Vector1(0.0)); + + DiscreteValues dv0{{M(1), 0}}; + DiscreteValues dv1{{M(1), 1}}; + + DiscreteConditional expected_m1(m1, "0.5/0.5"); + DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete()); + + EXPECT(assert_equal(expected_m1, actual_m1)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 1b5a6ebba9f9233698bc369ad4e4323927ccdd26 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 1 Nov 2024 14:31:40 -0400 Subject: [PATCH 18/21] update test name --- gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 13a6cd614..31e36101b 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -545,7 +545,7 @@ TEST(HybridGaussianFactorGraph, IncrementalErrorTree) { /* ****************************************************************************/ // Check that collectProductFactor works correctly. -TEST(HybridGaussianFactorGraph, collectProductFactor) { +TEST(HybridGaussianFactorGraph, CollectProductFactor) { const int num_measurements = 1; VectorValues vv{{Z(0), Vector1(5.0)}}; auto fg = tiny::createHybridGaussianFactorGraph(num_measurements, vv); From d091a9d4402abe7c717015abe1c68ed4707659be Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 1 Nov 2024 20:29:24 -0400 Subject: [PATCH 19/21] combined update and pruning --- gtsam/hybrid/tests/testHybridGaussianISAM.cpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 79669cfb3..0e323f5b4 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -258,7 +258,7 @@ TEST(HybridGaussianElimination, Approx_inference) { /* ****************************************************************************/ // Test approximate inference with an additional pruning step. -TEST_DISABLED(HybridGaussianElimination, IncrementalApproximate) { +TEST(HybridGaussianElimination, IncrementalApproximate) { Switching switching(5); HybridGaussianISAM incrementalHybrid; HybridGaussianFactorGraph graph; @@ -277,8 +277,7 @@ TEST_DISABLED(HybridGaussianElimination, IncrementalApproximate) { // Run update with pruning size_t maxComponents = 5; - incrementalHybrid.update(graph); - incrementalHybrid.prune(maxComponents); + incrementalHybrid.update(graph, maxComponents); // Check if we have a bayes tree with 4 hybrid nodes, // each with 2, 4, 8, and 5 (pruned) leaves respectively. @@ -298,8 +297,7 @@ TEST_DISABLED(HybridGaussianElimination, IncrementalApproximate) { graph.push_back(switching.linearUnaryFactors.at(4)); // x4 // Run update with pruning a second time. - incrementalHybrid.update(graph); - incrementalHybrid.prune(maxComponents); + incrementalHybrid.update(graph, maxComponents); // Check if we have a bayes tree with pruned hybrid nodes, // with 5 (pruned) leaves. @@ -470,8 +468,7 @@ TEST(HybridGaussianISAM, NonTrivial) { fg = HybridNonlinearFactorGraph(); // Keep pruning! - inc.update(gfg); - inc.prune(3); + inc.update(gfg, 3); // The final discrete graph should not be empty since we have eliminated // all continuous variables. From 6b3cb6579a6f1afb9f170ad6d7d5d3a1cd1c4735 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 2 Nov 2024 21:22:38 -0400 Subject: [PATCH 20/21] update test group name --- gtsam/hybrid/tests/testHybridGaussianISAM.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 0e323f5b4..04b44f904 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -56,7 +56,7 @@ const HybridGaussianFactorGraph graph2{lfg.at(4), lfg.at(1), lfg.at(2), /* ****************************************************************************/ // Test if we can perform elimination incrementally. -TEST(HybridGaussianElimination, IncrementalElimination) { +TEST(HybridGaussianISAM, IncrementalElimination) { using namespace switching3; HybridGaussianISAM isam; @@ -88,7 +88,7 @@ TEST(HybridGaussianElimination, IncrementalElimination) { /* ****************************************************************************/ // Test if we can incrementally do the inference -TEST(HybridGaussianElimination, IncrementalInference) { +TEST(HybridGaussianISAM, IncrementalInference) { using namespace switching3; HybridGaussianISAM isam; @@ -156,7 +156,7 @@ TEST(HybridGaussianElimination, IncrementalInference) { /* ****************************************************************************/ // Test if we can approximately do the inference -TEST(HybridGaussianElimination, Approx_inference) { +TEST(HybridGaussianISAM, ApproxInference) { Switching switching(4); HybridGaussianISAM incrementalHybrid; HybridGaussianFactorGraph graph1; @@ -258,7 +258,7 @@ TEST(HybridGaussianElimination, Approx_inference) { /* ****************************************************************************/ // Test approximate inference with an additional pruning step. -TEST(HybridGaussianElimination, IncrementalApproximate) { +TEST(HybridGaussianISAM, IncrementalApproximate) { Switching switching(5); HybridGaussianISAM incrementalHybrid; HybridGaussianFactorGraph graph; From a7b53aef0e5905d558560eab0fcb20db0832209e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 4 Nov 2024 14:52:21 -0500 Subject: [PATCH 21/21] better check for discrete factors --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 049e6c38d..b66847ca7 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -584,9 +584,9 @@ GaussianFactorGraph HybridGaussianFactorGraph::choose( DiscreteFactorGraph HybridGaussianFactorGraph::discreteFactors() const { DiscreteFactorGraph dfg; for (auto &&f : factors_) { - auto discreteFactor = std::dynamic_pointer_cast(f); - assert(discreteFactor); - dfg.push_back(discreteFactor); + if (auto discreteFactor = std::dynamic_pointer_cast(f)) { + dfg.push_back(discreteFactor); + } } return dfg; }