12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

get more nonlinear tests to work and make some updates

release/4.3a0
Varun Agrawal 2022-08-01 12:50:10 -04:00
parent 8471c97b9f
commit 89079229bc
3 changed files with 113 additions and 107 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/hybrid/HybridBayesTree.h
View File

@ -76,7 +76,10 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree<HybridBayesTreeClique> {
/** /**
* @brief Class for Hybrid Bayes tree orphan subtrees. * @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 * @tparam CLIQUE
*/ */

10
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -98,6 +98,12 @@ GaussianMixtureFactor::Sum sumFrontals(
} else if (f->isContinuous()) { } else if (f->isContinuous()) {
deferredFactors.push_back( deferredFactors.push_back(
boost::dynamic_pointer_cast<HybridGaussianFactor>(f)->inner()); boost::dynamic_pointer_cast<HybridGaussianFactor>(f)->inner());
} else if (f->isDiscrete()) {
// Don't do anything for discrete-only factors
// since we want to eliminate continuous values only.
continue;
} else { } else {
// We need to handle the case where the object is actually an // We need to handle the case where the object is actually an
// BayesTreeOrphanWrapper! // BayesTreeOrphanWrapper!
@ -106,8 +112,8 @@ GaussianMixtureFactor::Sum sumFrontals(
if (!orphan) { if (!orphan) {
auto &fr = *f; auto &fr = *f;
throw std::invalid_argument( throw std::invalid_argument(
std::string("factor is discrete in continuous elimination") + std::string("factor is discrete in continuous elimination ") +
typeid(fr).name()); demangle(typeid(fr).name()));
} }
} }
} }

205
gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
View File

@ -25,6 +25,8 @@
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h> #include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/hybrid/MixtureFactor.h> #include <gtsam/hybrid/MixtureFactor.h>
#include <gtsam/linear/GaussianBayesNet.h> #include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/PriorFactor.h> #include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/sam/BearingRangeFactor.h> #include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
@ -205,131 +207,126 @@ TEST(HybridFactorGraph, EliminationTree) {
EXPECT_LONGS_EQUAL(1, etree.roots().size()) EXPECT_LONGS_EQUAL(1, etree.roots().size())
} }
// /* /****************************************************************************
// ****************************************************************************/ *Test elimination function by eliminating x1 in *-x1-*-x2 graph.
// // Test elimination function by eliminating x1 in *-x1-*-x2 graph. */
// TEST(DCGaussianElimination, Eliminate_x1) { TEST(GaussianElimination, Eliminate_x1) {
// Switching self(3); Switching self(3);
// // Gather factors on x1, has a simple Gaussian and a mixture factor. // Gather factors on x1, has a simple Gaussian and a mixture factor.
// HybridGaussianFactorGraph factors; HybridGaussianFactorGraph factors;
// factors.push_gaussian(self.linearizedFactorGraph.gaussianGraph()[0]); // Add gaussian prior
// factors.push_dc(self.linearizedFactorGraph.dcGraph()[0]); factors.push_back(self.linearizedFactorGraph[0]);
// Add first hybrid factor
factors.push_back(self.linearizedFactorGraph[1]);
// // Check that sum works: // TODO(Varun) remove this block since sum is no longer exposed.
// auto sum = factors.sum(); // // Check that sum works:
// Assignment<Key> mode; // auto sum = factors.sum();
// mode[M(1)] = 1; // Assignment<Key> mode;
// auto actual = sum(mode); // Selects one of 2 modes. // mode[M(1)] = 1;
// EXPECT_LONGS_EQUAL(2, actual.size()); // Prior and motion model. // auto actual = sum(mode); // Selects one of 2 modes.
// EXPECT_LONGS_EQUAL(2, actual.size()); // Prior and motion model.
// // Eliminate x1 // Eliminate x1
// Ordering ordering; Ordering ordering;
// ordering += X(1); ordering += X(1);
// auto result = EliminateHybrid(factors, ordering); auto result = EliminateHybrid(factors, ordering);
// CHECK(result.first); CHECK(result.first);
// EXPECT_LONGS_EQUAL(1, result.first->nrFrontals()); EXPECT_LONGS_EQUAL(1, result.first->nrFrontals());
// CHECK(result.second); CHECK(result.second);
// // Has two keys, x2 and m1 // Has two keys, x2 and m1
// EXPECT_LONGS_EQUAL(2, result.second->size()); EXPECT_LONGS_EQUAL(2, result.second->size());
// } }
// /* /****************************************************************************
// ****************************************************************************/ * Test elimination function by eliminating x2 in x1-*-x2-*-x3 chain.
// // Test elimination function by eliminating x2 in x1-*-x2-*-x3 chain. * m1/ \m2
// // m1/ \m2 */
// TEST(DCGaussianElimination, Eliminate_x2) { TEST(HybridsGaussianElimination, Eliminate_x2) {
// Switching self(3); Switching self(3);
// // Gather factors on x2, will be two mixture factors (with x1 and x3, // Gather factors on x2, will be two mixture factors (with x1 and x3, resp.).
// resp.). HybridGaussianFactorGraph factors; HybridGaussianFactorGraph factors;
// factors.push_dc(self.linearizedFactorGraph.dcGraph()[0]); // involves m1 factors.push_back(self.linearizedFactorGraph[1]); // involves m1
// factors.push_dc(self.linearizedFactorGraph.dcGraph()[1]); // involves m2 factors.push_back(self.linearizedFactorGraph[2]); // involves m2
// // Check that sum works: // TODO(Varun) remove this block since sum is no longer exposed.
// auto sum = factors.sum(); // // Check that sum works:
// Assignment<Key> mode; // auto sum = factors.sum();
// mode[M(1)] = 0; // Assignment<Key> mode;
// mode[M(2)] = 1; // mode[M(1)] = 0;
// auto actual = sum(mode); // Selects one of 4 mode // mode[M(2)] = 1;
// combinations. EXPECT_LONGS_EQUAL(2, actual.size()); // 2 motion models. // auto actual = sum(mode); // Selects one of 4 mode
// combinations. EXPECT_LONGS_EQUAL(2, actual.size()); // 2 motion models.
// // Eliminate x2 // Eliminate x2
// Ordering ordering; Ordering ordering;
// ordering += X(2); ordering += X(2);
// std::pair<AbstractConditional::shared_ptr, boost::shared_ptr<Factor>> std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr> result =
// result = EliminateHybrid(factors, ordering);
// EliminateHybrid(factors, ordering); CHECK(result.first);
// CHECK(result.first); EXPECT_LONGS_EQUAL(1, result.first->nrFrontals());
// EXPECT_LONGS_EQUAL(1, result.first->nrFrontals()); CHECK(result.second);
// CHECK(result.second); // Note: separator keys should include m1, m2.
// // Note: separator keys should include m1, m2. EXPECT_LONGS_EQUAL(4, result.second->size());
// EXPECT_LONGS_EQUAL(4, result.second->size()); }
// }
// /* /*
// ****************************************************************************/ ****************************************************************************/
// // Helper method to generate gaussian factor graphs with a specific mode. // Helper method to generate gaussian factor graphs with a specific mode.
// GaussianFactorGraph::shared_ptr batchGFG(double between, GaussianFactorGraph::shared_ptr batchGFG(double between,
// Values linearizationPoint) { Values linearizationPoint) {
// NonlinearFactorGraph graph; NonlinearFactorGraph graph;
// graph.addPrior<double>(X(1), 0, Isotropic::Sigma(1, 0.1)); graph.addPrior<double>(X(1), 0, Isotropic::Sigma(1, 0.1));
// auto between_x1_x2 = boost::make_shared<MotionModel>( auto between_x1_x2 = boost::make_shared<MotionModel>(
// X(1), X(2), between, Isotropic::Sigma(1, 1.0)); 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 elimination function by eliminating x1 and x2 in graph. TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) {
// TEST(DCGaussianElimination, EliminateHybrid_2_Variable) { Switching self(2, 1.0, 0.1);
// Switching self(2, 1.0, 0.1);
// auto factors = self.linearizedFactorGraph; auto factors = self.linearizedFactorGraph;
// // Check that sum works: // Eliminate x1
// auto sum = factors.sum(); Ordering ordering;
// Assignment<Key> mode; ordering += X(1);
// mode[M(1)] = 1; ordering += X(2);
// auto actual = sum(mode); // Selects one of 2 modes.
// EXPECT_LONGS_EQUAL(4,
// actual.size()); // Prior, 1 motion models, 2
// measurements.
// // Eliminate x1 HybridConditional::shared_ptr hybridConditionalMixture;
// Ordering ordering; HybridFactor::shared_ptr factorOnModes;
// ordering += X(1);
// ordering += X(2);
// AbstractConditional::shared_ptr abstractConditionalMixture; std::tie(hybridConditionalMixture, factorOnModes) =
// boost::shared_ptr<Factor> factorOnModes; EliminateHybrid(factors, ordering);
// std::tie(abstractConditionalMixture, factorOnModes) =
// EliminateHybrid(factors, ordering);
// auto gaussianConditionalMixture = auto gaussianConditionalMixture =
// dynamic_pointer_cast<GaussianMixture>(abstractConditionalMixture); dynamic_pointer_cast<GaussianMixture>(hybridConditionalMixture->inner());
// CHECK(gaussianConditionalMixture); CHECK(gaussianConditionalMixture);
// EXPECT_LONGS_EQUAL( // Frontals = [x1, x2]
// 2, EXPECT_LONGS_EQUAL(2, gaussianConditionalMixture->nrFrontals());
// gaussianConditionalMixture->nrFrontals()); // Frontals = [x1, x2] // 1 parent, which is the mode
// EXPECT_LONGS_EQUAL( EXPECT_LONGS_EQUAL(1, gaussianConditionalMixture->nrParents());
// 1,
// gaussianConditionalMixture->nrParents()); // 1 parent, which is the
// mode
// auto discreteFactor = // This is now a HybridDiscreteFactor
// dynamic_pointer_cast<DecisionTreeFactor>(factorOnModes); auto hybridDiscreteFactor =
// CHECK(discreteFactor); dynamic_pointer_cast<HybridDiscreteFactor>(factorOnModes);
// EXPECT_LONGS_EQUAL(1, discreteFactor->discreteKeys().size()); // Access the type-erased inner object and convert to DecisionTreeFactor
// EXPECT(discreteFactor->root_->isLeaf() == false); auto discreteFactor =
// } dynamic_pointer_cast<DecisionTreeFactor>(hybridDiscreteFactor->inner());
CHECK(discreteFactor);
EXPECT_LONGS_EQUAL(1, discreteFactor->discreteKeys().size());
EXPECT(discreteFactor->root_->isLeaf() == false);
}
// /* // /*
// ****************************************************************************/ // ****************************************************************************/