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

remove unused blocks

release/4.3a0
Varun Agrawal 2022-09-02 21:30:10 -04:00
parent b0ba35d2bd
commit 9f2229fad5
1 changed files with 1 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -257,14 +257,6 @@ TEST(GaussianElimination, Eliminate_x1) {
// 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;
// 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);
@ -289,15 +281,6 @@ TEST(HybridsGaussianElimination, Eliminate_x2) {
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;
// 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);
@ -366,49 +349,6 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) {
EXPECT(discreteFactor->root_->isLeaf() == false);
}
// /*
// ****************************************************************************/
// /// Test the toDecisionTreeFactor method
// TEST(HybridFactorGraph, ToDecisionTreeFactor) {
// size_t K = 3;
// // Provide tight sigma values so that the errors are visibly different.
// double between_sigma = 5e-8, prior_sigma = 1e-7;
// Switching self(K, between_sigma, prior_sigma);
// // Clear out discrete factors since sum() cannot hanldle those
// HybridGaussianFactorGraph linearizedFactorGraph(
// self.linearizedFactorGraph.gaussianGraph(), DiscreteFactorGraph(),
// self.linearizedFactorGraph.dcGraph());
// auto decisionTreeFactor = linearizedFactorGraph.toDecisionTreeFactor();
// auto allAssignments =
// DiscreteValues::CartesianProduct(linearizedFactorGraph.discreteKeys());
// // Get the error of the discrete assignment m1=0, m2=1.
// double actual = (*decisionTreeFactor)(allAssignments[1]);
// /********************************************/
// // Create equivalent factor graph for m1=0, m2=1
// GaussianFactorGraph graph = linearizedFactorGraph.gaussianGraph();
// for (auto &p : linearizedFactorGraph.dcGraph()) {
// if (auto mixture =
// boost::dynamic_pointer_cast<DCGaussianMixtureFactor>(p)) {
// graph.add((*mixture)(allAssignments[1]));
// }
// }
// VectorValues values = graph.optimize();
// double expected = graph.probPrime(values);
// /********************************************/
// EXPECT_DOUBLES_EQUAL(expected, actual, 1e-12);
// // REGRESSION:
// EXPECT_DOUBLES_EQUAL(0.6125, actual, 1e-4);
// }
/****************************************************************************
* Test partial elimination
*/
@ -428,7 +368,6 @@ TEST(HybridFactorGraph, Partial_Elimination) {
linearizedFactorGraph.eliminatePartialSequential(ordering);
CHECK(hybridBayesNet);
// GTSAM_PRINT(*hybridBayesNet); // HybridBayesNet
EXPECT_LONGS_EQUAL(3, hybridBayesNet->size());
EXPECT(hybridBayesNet->at(0)->frontals() == KeyVector{X(1)});
EXPECT(hybridBayesNet->at(0)->parents() == KeyVector({X(2), M(1)}));
@ -438,7 +377,6 @@ TEST(HybridFactorGraph, Partial_Elimination) {
EXPECT(hybridBayesNet->at(2)->parents() == KeyVector({M(1), M(2)}));
CHECK(remainingFactorGraph);
// GTSAM_PRINT(*remainingFactorGraph); // HybridFactorGraph
EXPECT_LONGS_EQUAL(3, remainingFactorGraph->size());
EXPECT(remainingFactorGraph->at(0)->keys() == KeyVector({M(1)}));
EXPECT(remainingFactorGraph->at(1)->keys() == KeyVector({M(2), M(1)}));
@ -721,13 +659,8 @@ TEST(HybridFactorGraph, DefaultDecisionTree) {
moving = boost::make_shared<PlanarMotionModel>(X(0), X(1), odometry,
noise_model);
std::vector<PlanarMotionModel::shared_ptr> motion_models = {still, moving};
// TODO(Varun) Make a templated constructor for MixtureFactor which does this?
std::vector<NonlinearFactor::shared_ptr> components;
for (auto&& f : motion_models) {
components.push_back(boost::dynamic_pointer_cast<NonlinearFactor>(f));
}
fg.emplace_hybrid<MixtureFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, motion_models);
// Add Range-Bearing measurements to from X0 to L0 and X1 to L1.
// create a noise model for the landmark measurements