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.
*
* 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
*/

8
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -98,6 +98,12 @@ GaussianMixtureFactor::Sum sumFrontals(
} else if (f->isContinuous()) {
deferredFactors.push_back(
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 {
// We need to handle the case where the object is actually an
// BayesTreeOrphanWrapper!
@ -107,7 +113,7 @@ GaussianMixtureFactor::Sum sumFrontals(
auto &fr = *f;
throw std::invalid_argument(
std::string("factor is discrete in continuous elimination ") +
typeid(fr).name());
demangle(typeid(fr).name()));
}
}
}

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

@ -25,6 +25,8 @@
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/hybrid/MixtureFactor.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/slam/BetweenFactor.h>
@ -205,17 +207,20 @@ 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]);
// TODO(Varun) remove this block since sum is no longer exposed.
// // Check that sum works:
// auto sum = factors.sum();
// Assignment<Key> mode;
@ -223,30 +228,31 @@ TEST(HybridFactorGraph, EliminationTree) {
// 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
// TODO(Varun) remove this block since sum is no longer exposed.
// // Check that sum works:
// auto sum = factors.sum();
// Assignment<Key> mode;
@ -255,81 +261,72 @@ TEST(HybridFactorGraph, EliminationTree) {
// 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<AbstractConditional::shared_ptr, boost::shared_ptr<Factor>>
// 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<HybridConditional::shared_ptr, HybridFactor::shared_ptr> 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<double>(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<double>(X(1), 0, Isotropic::Sigma(1, 0.1));
// auto between_x1_x2 = boost::make_shared<MotionModel>(
// X(1), X(2), between, Isotropic::Sigma(1, 1.0));
auto between_x1_x2 = boost::make_shared<MotionModel>(
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<Key> 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<Factor> factorOnModes;
// std::tie(abstractConditionalMixture, factorOnModes) =
// EliminateHybrid(factors, ordering);
std::tie(hybridConditionalMixture, factorOnModes) =
EliminateHybrid(factors, ordering);
// auto gaussianConditionalMixture =
// dynamic_pointer_cast<GaussianMixture>(abstractConditionalMixture);
auto gaussianConditionalMixture =
dynamic_pointer_cast<GaussianMixture>(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<DecisionTreeFactor>(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<HybridDiscreteFactor>(factorOnModes);
// Access the type-erased inner object and convert to DecisionTreeFactor
auto discreteFactor =
dynamic_pointer_cast<DecisionTreeFactor>(hybridDiscreteFactor->inner());
CHECK(discreteFactor);
EXPECT_LONGS_EQUAL(1, discreteFactor->discreteKeys().size());
EXPECT(discreteFactor->root_->isLeaf() == false);
}
// /*
// ****************************************************************************/