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

get all but 2 tests passing

release/4.3a0
Varun Agrawal 2022-08-02 19:01:49 -04:00
parent 89079229bc
commit 16124f3617
1 changed files with 279 additions and 266 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -19,6 +19,7 @@
#include <gtsam/base/utilities.h> #include <gtsam/base/utilities.h>
#include <gtsam/discrete/DiscreteBayesNet.h> #include <gtsam/discrete/DiscreteBayesNet.h>
#include <gtsam/discrete/DiscreteDistribution.h> #include <gtsam/discrete/DiscreteDistribution.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/hybrid/HybridEliminationTree.h> #include <gtsam/hybrid/HybridEliminationTree.h>
#include <gtsam/hybrid/HybridFactor.h> #include <gtsam/hybrid/HybridFactor.h>
@ -274,9 +275,9 @@ TEST(HybridsGaussianElimination, Eliminate_x2) {
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;
@ -290,8 +291,9 @@ GaussianFactorGraph::shared_ptr batchGFG(double between,
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(HybridGaussianElimination, EliminateHybrid_2_Variable) {
Switching self(2, 1.0, 0.1); Switching self(2, 1.0, 0.1);
@ -371,287 +373,298 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) {
// EXPECT_DOUBLES_EQUAL(0.6125, actual, 1e-4); // EXPECT_DOUBLES_EQUAL(0.6125, actual, 1e-4);
// } // }
// /* /****************************************************************************
// ****************************************************************************/ * Test partial elimination
// // Test partial elimination */
// TEST_UNSAFE(HybridFactorGraph, Partial_Elimination) { TEST(HybridFactorGraph, Partial_Elimination) {
// Switching self(3); Switching self(3);
// auto linearizedFactorGraph = self.linearizedFactorGraph; auto linearizedFactorGraph = self.linearizedFactorGraph;
// // Create ordering. // Create ordering.
// Ordering ordering; Ordering ordering;
// for (size_t k = 1; k <= self.K; k++) ordering += X(k); for (size_t k = 1; k <= self.K; k++) ordering += X(k);
// // Eliminate partially. // Eliminate partially.
// HybridBayesNet::shared_ptr hybridBayesNet; HybridBayesNet::shared_ptr hybridBayesNet;
// HybridGaussianFactorGraph::shared_ptr remainingFactorGraph; HybridGaussianFactorGraph::shared_ptr remainingFactorGraph;
// std::tie(hybridBayesNet, remainingFactorGraph) = std::tie(hybridBayesNet, remainingFactorGraph) =
// linearizedFactorGraph.eliminatePartialSequential(ordering); linearizedFactorGraph.eliminatePartialSequential(ordering);
// CHECK(hybridBayesNet); CHECK(hybridBayesNet);
// // GTSAM_PRINT(*hybridBayesNet); // HybridBayesNet // GTSAM_PRINT(*hybridBayesNet); // HybridBayesNet
// EXPECT_LONGS_EQUAL(3, hybridBayesNet->size()); EXPECT_LONGS_EQUAL(3, hybridBayesNet->size());
// EXPECT(hybridBayesNet->at(0)->frontals() == KeyVector{X(1)}); EXPECT(hybridBayesNet->at(0)->frontals() == KeyVector{X(1)});
// EXPECT(hybridBayesNet->at(0)->parents() == KeyVector({X(2), M(1)})); EXPECT(hybridBayesNet->at(0)->parents() == KeyVector({X(2), M(1)}));
// EXPECT(hybridBayesNet->at(1)->frontals() == KeyVector{X(2)}); EXPECT(hybridBayesNet->at(1)->frontals() == KeyVector{X(2)});
// EXPECT(hybridBayesNet->at(1)->parents() == KeyVector({X(3), M(2), M(1)})); EXPECT(hybridBayesNet->at(1)->parents() == KeyVector({X(3), M(1), M(2)}));
// EXPECT(hybridBayesNet->at(2)->frontals() == KeyVector{X(3)}); EXPECT(hybridBayesNet->at(2)->frontals() == KeyVector{X(3)});
// EXPECT(hybridBayesNet->at(2)->parents() == KeyVector({M(2), M(1)})); EXPECT(hybridBayesNet->at(2)->parents() == KeyVector({M(1), M(2)}));
// CHECK(remainingFactorGraph); CHECK(remainingFactorGraph);
// // GTSAM_PRINT(*remainingFactorGraph); // HybridFactorGraph // GTSAM_PRINT(*remainingFactorGraph); // HybridFactorGraph
// EXPECT_LONGS_EQUAL(3, remainingFactorGraph->size()); EXPECT_LONGS_EQUAL(3, remainingFactorGraph->size());
// EXPECT(remainingFactorGraph->discreteGraph().at(0)->keys() == EXPECT(remainingFactorGraph->at(0)->keys() == KeyVector({M(1)}));
// KeyVector({M(1)})); EXPECT(remainingFactorGraph->at(1)->keys() == KeyVector({M(2), M(1)}));
// EXPECT(remainingFactorGraph->discreteGraph().at(1)->keys() == EXPECT(remainingFactorGraph->at(2)->keys() == KeyVector({M(1), M(2)}));
// KeyVector({M(2), M(1)})); }
// EXPECT(remainingFactorGraph->discreteGraph().at(2)->keys() ==
// KeyVector({M(2), M(1)}));
// }
// /* /****************************************************************************
// ****************************************************************************/ * Test full elimination
// // Test full elimination */
// TEST_UNSAFE(HybridFactorGraph, Full_Elimination) { TEST(HybridFactorGraph, Full_Elimination) {
// Switching self(3); Switching self(3);
// auto linearizedFactorGraph = self.linearizedFactorGraph; auto linearizedFactorGraph = self.linearizedFactorGraph;
// // We first do a partial elimination // We first do a partial elimination
// HybridBayesNet::shared_ptr hybridBayesNet_partial; HybridBayesNet::shared_ptr hybridBayesNet_partial;
// HybridGaussianFactorGraph::shared_ptr remainingFactorGraph_partial; HybridGaussianFactorGraph::shared_ptr remainingFactorGraph_partial;
// DiscreteBayesNet discreteBayesNet; DiscreteBayesNet discreteBayesNet;
// { {
// // Create ordering. // Create ordering.
// Ordering ordering; Ordering ordering;
// for (size_t k = 1; k <= self.K; k++) ordering += X(k); for (size_t k = 1; k <= self.K; k++) ordering += X(k);
// // Eliminate partially. // Eliminate partially.
// std::tie(hybridBayesNet_partial, remainingFactorGraph_partial) = std::tie(hybridBayesNet_partial, remainingFactorGraph_partial) =
// linearizedFactorGraph.eliminatePartialSequential(ordering); linearizedFactorGraph.eliminatePartialSequential(ordering);
// DiscreteFactorGraph dfg; DiscreteFactorGraph discrete_fg;
// dfg.push_back(remainingFactorGraph_partial->discreteGraph()); //TODO(Varun) Make this a function of HybridGaussianFactorGraph?
// ordering.clear(); for(HybridFactor::shared_ptr& factor: (*remainingFactorGraph_partial)) {
// for (size_t k = 1; k < self.K; k++) ordering += M(k); auto df = dynamic_pointer_cast<HybridDiscreteFactor>(factor);
// discreteBayesNet = *dfg.eliminateSequential(ordering, EliminateForMPE); discrete_fg.push_back(df->inner());
// } }
ordering.clear();
for (size_t k = 1; k < self.K; k++) ordering += M(k);
discreteBayesNet = *discrete_fg.eliminateSequential(ordering, EliminateForMPE);
}
// // Create ordering. // Create ordering.
// Ordering ordering; Ordering ordering;
// for (size_t k = 1; k <= self.K; k++) ordering += X(k); for (size_t k = 1; k <= self.K; k++) ordering += X(k);
// for (size_t k = 1; k < self.K; k++) ordering += M(k); for (size_t k = 1; k < self.K; k++) ordering += M(k);
// // Eliminate partially. // Eliminate partially.
// HybridBayesNet::shared_ptr hybridBayesNet = HybridBayesNet::shared_ptr hybridBayesNet =
// linearizedFactorGraph.eliminateSequential(ordering); linearizedFactorGraph.eliminateSequential(ordering);
// CHECK(hybridBayesNet); CHECK(hybridBayesNet);
// EXPECT_LONGS_EQUAL(5, hybridBayesNet->size()); EXPECT_LONGS_EQUAL(5, hybridBayesNet->size());
// // p(x1 | x2, m1) // p(x1 | x2, m1)
// EXPECT(hybridBayesNet->at(0)->frontals() == KeyVector{X(1)}); EXPECT(hybridBayesNet->at(0)->frontals() == KeyVector{X(1)});
// EXPECT(hybridBayesNet->at(0)->parents() == KeyVector({X(2), M(1)})); EXPECT(hybridBayesNet->at(0)->parents() == KeyVector({X(2), M(1)}));
// // p(x2 | x3, m1, m2) // p(x2 | x3, m1, m2)
// EXPECT(hybridBayesNet->at(1)->frontals() == KeyVector{X(2)}); EXPECT(hybridBayesNet->at(1)->frontals() == KeyVector{X(2)});
// EXPECT(hybridBayesNet->at(1)->parents() == KeyVector({X(3), M(2), M(1)})); EXPECT(hybridBayesNet->at(1)->parents() == KeyVector({X(3), M(1), M(2)}));
// // p(x3 | m1, m2) // p(x3 | m1, m2)
// EXPECT(hybridBayesNet->at(2)->frontals() == KeyVector{X(3)}); EXPECT(hybridBayesNet->at(2)->frontals() == KeyVector{X(3)});
// EXPECT(hybridBayesNet->at(2)->parents() == KeyVector({M(2), M(1)})); EXPECT(hybridBayesNet->at(2)->parents() == KeyVector({M(1), M(2)}));
// // P(m1 | m2) // P(m1 | m2)
// EXPECT(hybridBayesNet->at(3)->frontals() == KeyVector{M(1)}); EXPECT(hybridBayesNet->at(3)->frontals() == KeyVector{M(1)});
// EXPECT(hybridBayesNet->at(3)->parents() == KeyVector({M(2)})); EXPECT(hybridBayesNet->at(3)->parents() == KeyVector({M(2)}));
// EXPECT(dynamic_pointer_cast<DiscreteConditional>(hybridBayesNet->at(3)) GTSAM_PRINT(*(hybridBayesNet->at(3)));
GTSAM_PRINT(*(discreteBayesNet.at(0)));
//TODO(Varun) FIX!!
// EXPECT(
// dynamic_pointer_cast<DiscreteConditional>(hybridBayesNet->at(3)->inner())
// ->equals(*discreteBayesNet.at(0))); // ->equals(*discreteBayesNet.at(0)));
// // P(m2) // // P(m2)
// EXPECT(hybridBayesNet->at(4)->frontals() == KeyVector{M(2)}); // EXPECT(hybridBayesNet->at(4)->frontals() == KeyVector{M(2)});
// EXPECT_LONGS_EQUAL(0, hybridBayesNet->at(4)->nrParents()); // EXPECT_LONGS_EQUAL(0, hybridBayesNet->at(4)->nrParents());
// EXPECT(dynamic_pointer_cast<DiscreteConditional>(hybridBayesNet->at(4)) // EXPECT(
// dynamic_pointer_cast<DiscreteConditional>(hybridBayesNet->at(4)->inner())
// ->equals(*discreteBayesNet.at(1))); // ->equals(*discreteBayesNet.at(1)));
// } }
// /* /*
// ****************************************************************************/ ****************************************************************************/
// // Test printing // Test printing
// TEST(HybridFactorGraph, Printing) { TEST(HybridFactorGraph, Printing) {
// Switching self(3); Switching self(3);
// auto linearizedFactorGraph = self.linearizedFactorGraph; auto linearizedFactorGraph = self.linearizedFactorGraph;
// // Create ordering. // Create ordering.
// Ordering ordering; Ordering ordering;
// for (size_t k = 1; k <= self.K; k++) ordering += X(k); for (size_t k = 1; k <= self.K; k++) ordering += X(k);
// // Eliminate partially. // Eliminate partially.
// HybridBayesNet::shared_ptr hybridBayesNet; HybridBayesNet::shared_ptr hybridBayesNet;
// HybridGaussianFactorGraph::shared_ptr remainingFactorGraph; HybridGaussianFactorGraph::shared_ptr remainingFactorGraph;
// std::tie(hybridBayesNet, remainingFactorGraph) = std::tie(hybridBayesNet, remainingFactorGraph) =
// linearizedFactorGraph.eliminatePartialSequential(ordering); linearizedFactorGraph.eliminatePartialSequential(ordering);
// string expected_hybridFactorGraph = R"( string expected_hybridFactorGraph = R"(
// size: 8 size: 8
// DiscreteFactorGraph factor 0: Continuous x1;
// size: 2
// factor 0: P( m1 ):
// Leaf 0.5
// factor 1: P( m2 | m1 ):
// Choice(m2)
// 0 Choice(m1)
// 0 0 Leaf 0.33333333
// 0 1 Leaf 0.6
// 1 Choice(m1)
// 1 0 Leaf 0.66666667
// 1 1 Leaf 0.4
// DCFactorGraph
// size: 2
// factor 0: [ x1 x2; m1 ]{
// Choice(m1)
// 0 Leaf Jacobian factor on 2 keys:
// A[x1] = [
// -1
// ]
// A[x2] = [
// 1
// ]
// b = [ -1 ]
// No noise model
// 1 Leaf Jacobian factor on 2 keys:
// A[x1] = [
// -1
// ]
// A[x2] = [
// 1
// ]
// b = [ -0 ]
// No noise model
// }
// factor 1: [ x2 x3; m2 ]{
// Choice(m2)
// 0 Leaf Jacobian factor on 2 keys:
// A[x2] = [
// -1
// ]
// A[x3] = [
// 1
// ]
// b = [ -1 ]
// No noise model
// 1 Leaf Jacobian factor on 2 keys:
// A[x2] = [
// -1
// ]
// A[x3] = [
// 1
// ]
// b = [ -0 ]
// No noise model
// }
// GaussianGraph
// size: 4
// factor 0:
// A[x1] = [
// 10
// ]
// b = [ -10 ]
// No noise model
// factor 1:
// A[x1] = [
// 10
// ]
// b = [ -10 ]
// No noise model
// factor 2:
// A[x2] = [
// 10
// ]
// b = [ -10 ]
// No noise model
// factor 3:
// A[x3] = [
// 10
// ]
// b = [ -10 ]
// No noise model
// )";
// EXPECT(assert_print_equal(expected_hybridFactorGraph,
// linearizedFactorGraph));
// // Expected output for hybridBayesNet. A[x1] = [
// string expected_hybridBayesNet = R"( 10
// size: 3 ]
// factor 0: GaussianMixture [ x1 | x2 m1 ]{ b = [ -10 ]
// Choice(m1) No noise model
// 0 Leaf Jacobian factor on 2 keys: factor 1: Hybrid x1 x2 m1; m1 ]{
// p(x1 | x2) Choice(m1)
// R = [ 14.1774 ] 0 Leaf :
// S[x2] = [ -0.0705346 ] A[x1] = [
// d = [ -14.0364 ] -1
// No noise model ]
// 1 Leaf Jacobian factor on 2 keys: A[x2] = [
// p(x1 | x2) 1
// R = [ 14.1774 ] ]
// S[x2] = [ -0.0705346 ] b = [ -1 ]
// d = [ -14.1069 ] No noise model
// No noise model
// } 1 Leaf :
// factor 1: GaussianMixture [ x2 | x3 m2 m1 ]{ A[x1] = [
// Choice(m2) -1
// 0 Choice(m1) ]
// 0 0 Leaf Jacobian factor on 2 keys: A[x2] = [
// p(x2 | x3) 1
// R = [ 10.0993 ] ]
// S[x3] = [ -0.0990172 ] b = [ -0 ]
// d = [ -9.99975 ] No noise model
// No noise model
// 0 1 Leaf Jacobian factor on 2 keys: }
// p(x2 | x3) factor 2: Hybrid x2 x3 m2; m2 ]{
// R = [ 10.0993 ] Choice(m2)
// S[x3] = [ -0.0990172 ] 0 Leaf :
// d = [ -9.90122 ] A[x2] = [
// No noise model -1
// 1 Choice(m1) ]
// 1 0 Leaf Jacobian factor on 2 keys: A[x3] = [
// p(x2 | x3) 1
// R = [ 10.0993 ] ]
// S[x3] = [ -0.0990172 ] b = [ -1 ]
// d = [ -10.0988 ] No noise model
// No noise model
// 1 1 Leaf Jacobian factor on 2 keys: 1 Leaf :
// p(x2 | x3) A[x2] = [
// R = [ 10.0993 ] -1
// S[x3] = [ -0.0990172 ] ]
// d = [ -10.0002 ] A[x3] = [
// No noise model 1
// } ]
// factor 2: GaussianMixture [ x3 | m2 m1 ]{ b = [ -0 ]
// Choice(m2) No noise model
// 0 Choice(m1)
// 0 0 Leaf Jacobian factor on 1 keys: }
// p(x3) factor 3: Continuous x1;
// R = [ 10.0494 ]
// d = [ -10.1489 ] A[x1] = [
// No noise model 10
// 0 1 Leaf Jacobian factor on 1 keys: ]
// p(x3) b = [ -10 ]
// R = [ 10.0494 ] No noise model
// d = [ -10.1479 ] factor 4: Continuous x2;
// No noise model
// 1 Choice(m1) A[x2] = [
// 1 0 Leaf Jacobian factor on 1 keys: 10
// p(x3) ]
// R = [ 10.0494 ] b = [ -10 ]
// d = [ -10.0504 ] No noise model
// No noise model factor 5: Continuous x3;
// 1 1 Leaf Jacobian factor on 1 keys:
// p(x3) A[x3] = [
// R = [ 10.0494 ] 10
// d = [ -10.0494 ] ]
// No noise model b = [ -10 ]
// } No noise model
// )"; factor 6: Discrete m1
// EXPECT(assert_print_equal(expected_hybridBayesNet, *hybridBayesNet)); P( m1 ):
// } Leaf 0.5
factor 7: Discrete m2 m1
P( m2 | m1 ):
Choice(m2)
0 Choice(m1)
0 0 Leaf 0.33333333
0 1 Leaf 0.6
1 Choice(m1)
1 0 Leaf 0.66666667
1 1 Leaf 0.4
)";
EXPECT(assert_print_equal(expected_hybridFactorGraph, linearizedFactorGraph));
// Expected output for hybridBayesNet.
string expected_hybridBayesNet = R"(
size: 3
factor 0: Hybrid P( x1 | x2 m1)
Discrete Keys = (m1, 2),
Choice(m1)
0 Leaf p(x1 | x2)
R = [ 14.1774 ]
S[x2] = [ -0.0705346 ]
d = [ -14.0364 ]
No noise model
1 Leaf p(x1 | x2)
R = [ 14.1774 ]
S[x2] = [ -0.0705346 ]
d = [ -14.1069 ]
No noise model
factor 1: Hybrid P( x2 | x3 m1 m2)
Discrete Keys = (m1, 2), (m2, 2),
Choice(m2)
0 Choice(m1)
0 0 Leaf p(x2 | x3)
R = [ 10.0993 ]
S[x3] = [ -0.0990172 ]
d = [ -9.99975 ]
No noise model
0 1 Leaf p(x2 | x3)
R = [ 10.0993 ]
S[x3] = [ -0.0990172 ]
d = [ -9.90122 ]
No noise model
1 Choice(m1)
1 0 Leaf p(x2 | x3)
R = [ 10.0993 ]
S[x3] = [ -0.0990172 ]
d = [ -10.0988 ]
No noise model
1 1 Leaf p(x2 | x3)
R = [ 10.0993 ]
S[x3] = [ -0.0990172 ]
d = [ -10.0002 ]
No noise model
factor 2: Hybrid P( x3 | m1 m2)
Discrete Keys = (m1, 2), (m2, 2),
Choice(m2)
0 Choice(m1)
0 0 Leaf p(x3)
R = [ 10.0494 ]
d = [ -10.1489 ]
No noise model
0 1 Leaf p(x3)
R = [ 10.0494 ]
d = [ -10.1479 ]
No noise model
1 Choice(m1)
1 0 Leaf p(x3)
R = [ 10.0494 ]
d = [ -10.0504 ]
No noise model
1 1 Leaf p(x3)
R = [ 10.0494 ]
d = [ -10.0494 ]
No noise model
)";
EXPECT(assert_print_equal(expected_hybridBayesNet, *hybridBayesNet));
}
// /* ************************************************************************* // /* *************************************************************************
// */ // */