From 89079229bcd02f6cc1d3df829a225c853848f259 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 1 Aug 2022 12:50:10 -0400 Subject: [PATCH] get more nonlinear tests to work and make some updates --- gtsam/hybrid/HybridBayesTree.h | 5 +- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 10 +- .../tests/testHybridNonlinearFactorGraph.cpp | 205 +++++++++--------- 3 files changed, 113 insertions(+), 107 deletions(-) diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index 0b89ca8c4..02a4a11e5 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -76,7 +76,10 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree { /** * @brief Class for Hybrid Bayes tree orphan subtrees. * - * This does special stuff for the hybrid case + * This object stores parent keys in our base type factor so that + * eliminating those parent keys will pull this subtree into the + * elimination. + * This does special stuff for the hybrid case. * * @tparam CLIQUE */ diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 88730cae9..3536f6a36 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -98,6 +98,12 @@ GaussianMixtureFactor::Sum sumFrontals( } else if (f->isContinuous()) { deferredFactors.push_back( boost::dynamic_pointer_cast(f)->inner()); + + } else if (f->isDiscrete()) { + // Don't do anything for discrete-only factors + // since we want to eliminate continuous values only. + continue; + } else { // We need to handle the case where the object is actually an // BayesTreeOrphanWrapper! @@ -106,8 +112,8 @@ GaussianMixtureFactor::Sum sumFrontals( if (!orphan) { auto &fr = *f; throw std::invalid_argument( - std::string("factor is discrete in continuous elimination") + - typeid(fr).name()); + std::string("factor is discrete in continuous elimination ") + + demangle(typeid(fr).name())); } } } diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index fb0778ed8..1f8abc4f6 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -25,6 +25,8 @@ #include #include #include +#include +#include #include #include #include @@ -205,131 +207,126 @@ TEST(HybridFactorGraph, EliminationTree) { EXPECT_LONGS_EQUAL(1, etree.roots().size()) } -// /* -// ****************************************************************************/ -// // Test elimination function by eliminating x1 in *-x1-*-x2 graph. -// TEST(DCGaussianElimination, Eliminate_x1) { -// Switching self(3); +/**************************************************************************** + *Test elimination function by eliminating x1 in *-x1-*-x2 graph. + */ +TEST(GaussianElimination, Eliminate_x1) { + Switching self(3); -// // Gather factors on x1, has a simple Gaussian and a mixture factor. -// HybridGaussianFactorGraph factors; -// factors.push_gaussian(self.linearizedFactorGraph.gaussianGraph()[0]); -// factors.push_dc(self.linearizedFactorGraph.dcGraph()[0]); + // Gather factors on x1, has a simple Gaussian and a mixture factor. + HybridGaussianFactorGraph factors; + // Add gaussian prior + factors.push_back(self.linearizedFactorGraph[0]); + // Add first hybrid factor + factors.push_back(self.linearizedFactorGraph[1]); -// // Check that sum works: -// auto sum = factors.sum(); -// Assignment mode; -// mode[M(1)] = 1; -// auto actual = sum(mode); // Selects one of 2 modes. -// EXPECT_LONGS_EQUAL(2, actual.size()); // Prior and motion model. + // TODO(Varun) remove this block since sum is no longer exposed. + // // Check that sum works: + // auto sum = factors.sum(); + // Assignment mode; + // mode[M(1)] = 1; + // auto actual = sum(mode); // Selects one of 2 modes. + // EXPECT_LONGS_EQUAL(2, actual.size()); // Prior and motion model. -// // Eliminate x1 -// Ordering ordering; -// ordering += X(1); + // Eliminate x1 + Ordering ordering; + ordering += X(1); -// auto result = EliminateHybrid(factors, ordering); -// CHECK(result.first); -// EXPECT_LONGS_EQUAL(1, result.first->nrFrontals()); -// CHECK(result.second); -// // Has two keys, x2 and m1 -// EXPECT_LONGS_EQUAL(2, result.second->size()); -// } + auto result = EliminateHybrid(factors, ordering); + CHECK(result.first); + EXPECT_LONGS_EQUAL(1, result.first->nrFrontals()); + CHECK(result.second); + // Has two keys, x2 and m1 + EXPECT_LONGS_EQUAL(2, result.second->size()); +} -// /* -// ****************************************************************************/ -// // Test elimination function by eliminating x2 in x1-*-x2-*-x3 chain. -// // m1/ \m2 -// TEST(DCGaussianElimination, Eliminate_x2) { -// Switching self(3); +/**************************************************************************** + * Test elimination function by eliminating x2 in x1-*-x2-*-x3 chain. + * m1/ \m2 + */ +TEST(HybridsGaussianElimination, Eliminate_x2) { + Switching self(3); -// // Gather factors on x2, will be two mixture factors (with x1 and x3, -// resp.). HybridGaussianFactorGraph factors; -// factors.push_dc(self.linearizedFactorGraph.dcGraph()[0]); // involves m1 -// factors.push_dc(self.linearizedFactorGraph.dcGraph()[1]); // involves m2 + // Gather factors on x2, will be two mixture factors (with x1 and x3, resp.). + HybridGaussianFactorGraph factors; + factors.push_back(self.linearizedFactorGraph[1]); // involves m1 + factors.push_back(self.linearizedFactorGraph[2]); // involves m2 -// // Check that sum works: -// auto sum = factors.sum(); -// Assignment mode; -// mode[M(1)] = 0; -// mode[M(2)] = 1; -// auto actual = sum(mode); // Selects one of 4 mode -// combinations. EXPECT_LONGS_EQUAL(2, actual.size()); // 2 motion models. + // TODO(Varun) remove this block since sum is no longer exposed. + // // Check that sum works: + // auto sum = factors.sum(); + // Assignment mode; + // mode[M(1)] = 0; + // mode[M(2)] = 1; + // auto actual = sum(mode); // Selects one of 4 mode + // combinations. EXPECT_LONGS_EQUAL(2, actual.size()); // 2 motion models. -// // Eliminate x2 -// Ordering ordering; -// ordering += X(2); + // Eliminate x2 + Ordering ordering; + ordering += X(2); -// std::pair> -// result = -// EliminateHybrid(factors, ordering); -// CHECK(result.first); -// EXPECT_LONGS_EQUAL(1, result.first->nrFrontals()); -// CHECK(result.second); -// // Note: separator keys should include m1, m2. -// EXPECT_LONGS_EQUAL(4, result.second->size()); -// } + std::pair result = + EliminateHybrid(factors, ordering); + CHECK(result.first); + EXPECT_LONGS_EQUAL(1, result.first->nrFrontals()); + CHECK(result.second); + // Note: separator keys should include m1, m2. + EXPECT_LONGS_EQUAL(4, result.second->size()); +} -// /* -// ****************************************************************************/ -// // Helper method to generate gaussian factor graphs with a specific mode. -// GaussianFactorGraph::shared_ptr batchGFG(double between, -// Values linearizationPoint) { -// NonlinearFactorGraph graph; -// graph.addPrior(X(1), 0, Isotropic::Sigma(1, 0.1)); +/* +****************************************************************************/ +// Helper method to generate gaussian factor graphs with a specific mode. +GaussianFactorGraph::shared_ptr batchGFG(double between, + Values linearizationPoint) { + NonlinearFactorGraph graph; + graph.addPrior(X(1), 0, Isotropic::Sigma(1, 0.1)); -// auto between_x1_x2 = boost::make_shared( -// X(1), X(2), between, Isotropic::Sigma(1, 1.0)); + auto between_x1_x2 = boost::make_shared( + X(1), X(2), between, Isotropic::Sigma(1, 1.0)); -// graph.push_back(between_x1_x2); + graph.push_back(between_x1_x2); -// return graph.linearize(linearizationPoint); -// } + return graph.linearize(linearizationPoint); +} -// /* -// ****************************************************************************/ -// // Test elimination function by eliminating x1 and x2 in graph. -// TEST(DCGaussianElimination, EliminateHybrid_2_Variable) { -// Switching self(2, 1.0, 0.1); +/*****************************************************************************/ +// Test elimination function by eliminating x1 and x2 in graph. +TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) { + Switching self(2, 1.0, 0.1); -// auto factors = self.linearizedFactorGraph; + auto factors = self.linearizedFactorGraph; -// // Check that sum works: -// auto sum = factors.sum(); -// Assignment mode; -// mode[M(1)] = 1; -// auto actual = sum(mode); // Selects one of 2 modes. -// EXPECT_LONGS_EQUAL(4, -// actual.size()); // Prior, 1 motion models, 2 -// measurements. + // Eliminate x1 + Ordering ordering; + ordering += X(1); + ordering += X(2); -// // Eliminate x1 -// Ordering ordering; -// ordering += X(1); -// ordering += X(2); + HybridConditional::shared_ptr hybridConditionalMixture; + HybridFactor::shared_ptr factorOnModes; -// AbstractConditional::shared_ptr abstractConditionalMixture; -// boost::shared_ptr factorOnModes; -// std::tie(abstractConditionalMixture, factorOnModes) = -// EliminateHybrid(factors, ordering); + std::tie(hybridConditionalMixture, factorOnModes) = + EliminateHybrid(factors, ordering); -// auto gaussianConditionalMixture = -// dynamic_pointer_cast(abstractConditionalMixture); + auto gaussianConditionalMixture = + dynamic_pointer_cast(hybridConditionalMixture->inner()); -// CHECK(gaussianConditionalMixture); -// EXPECT_LONGS_EQUAL( -// 2, -// gaussianConditionalMixture->nrFrontals()); // Frontals = [x1, x2] -// EXPECT_LONGS_EQUAL( -// 1, -// gaussianConditionalMixture->nrParents()); // 1 parent, which is the -// mode + CHECK(gaussianConditionalMixture); + // Frontals = [x1, x2] + EXPECT_LONGS_EQUAL(2, gaussianConditionalMixture->nrFrontals()); + // 1 parent, which is the mode + EXPECT_LONGS_EQUAL(1, gaussianConditionalMixture->nrParents()); -// auto discreteFactor = -// dynamic_pointer_cast(factorOnModes); -// CHECK(discreteFactor); -// EXPECT_LONGS_EQUAL(1, discreteFactor->discreteKeys().size()); -// EXPECT(discreteFactor->root_->isLeaf() == false); -// } + // This is now a HybridDiscreteFactor + auto hybridDiscreteFactor = + dynamic_pointer_cast(factorOnModes); + // Access the type-erased inner object and convert to DecisionTreeFactor + auto discreteFactor = + dynamic_pointer_cast(hybridDiscreteFactor->inner()); + CHECK(discreteFactor); + EXPECT_LONGS_EQUAL(1, discreteFactor->discreteKeys().size()); + EXPECT(discreteFactor->root_->isLeaf() == false); +} // /* // ****************************************************************************/