diff --git a/gtsam/inference/tests/testFactorGraph.cpp b/gtsam/inference/tests/testFactorGraph.cpp index b2f9bc3f1..bfbe716d4 100644 --- a/gtsam/inference/tests/testFactorGraph.cpp +++ b/gtsam/inference/tests/testFactorGraph.cpp @@ -60,65 +60,6 @@ TEST(FactorGraph, eliminateFrontals) { EXPECT(assert_equal(expectedCond, *actualCond)); } -///* ************************************************************************* */ -// SL-FIX TEST( FactorGraph, splitMinimumSpanningTree ) -//{ -// SymbolicFactorGraph G; -// G.push_factor("x1", "x2"); -// G.push_factor("x1", "x3"); -// G.push_factor("x1", "x4"); -// G.push_factor("x2", "x3"); -// G.push_factor("x2", "x4"); -// G.push_factor("x3", "x4"); -// -// SymbolicFactorGraph T, C; -// boost::tie(T, C) = G.splitMinimumSpanningTree(); -// -// SymbolicFactorGraph expectedT, expectedC; -// expectedT.push_factor("x1", "x2"); -// expectedT.push_factor("x1", "x3"); -// expectedT.push_factor("x1", "x4"); -// expectedC.push_factor("x2", "x3"); -// expectedC.push_factor("x2", "x4"); -// expectedC.push_factor("x3", "x4"); -// CHECK(assert_equal(expectedT,T)); -// CHECK(assert_equal(expectedC,C)); -//} - -///* ************************************************************************* */ -///** -// * x1 - x2 - x3 - x4 - x5 -// * | | / | -// * l1 l2 l3 -// */ -// SL-FIX TEST( FactorGraph, removeSingletons ) -//{ -// SymbolicFactorGraph G; -// G.push_factor("x1", "x2"); -// G.push_factor("x2", "x3"); -// G.push_factor("x3", "x4"); -// G.push_factor("x4", "x5"); -// G.push_factor("x2", "l1"); -// G.push_factor("x3", "l2"); -// G.push_factor("x4", "l2"); -// G.push_factor("x4", "l3"); -// -// SymbolicFactorGraph singletonGraph; -// set singletons; -// boost::tie(singletonGraph, singletons) = G.removeSingletons(); -// -// set singletons_excepted; singletons_excepted += "x1", "x2", "x5", "l1", "l3"; -// CHECK(singletons_excepted == singletons); -// -// SymbolicFactorGraph singletonGraph_excepted; -// singletonGraph_excepted.push_factor("x2", "l1"); -// singletonGraph_excepted.push_factor("x4", "l3"); -// singletonGraph_excepted.push_factor("x1", "x2"); -// singletonGraph_excepted.push_factor("x4", "x5"); -// singletonGraph_excepted.push_factor("x2", "x3"); -// CHECK(singletonGraph_excepted.equals(singletonGraph)); -//} - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/tests/testGaussianFactor.cpp b/tests/testGaussianFactor.cpp index 0097ec49a..3fe92c50d 100644 --- a/tests/testGaussianFactor.cpp +++ b/tests/testGaussianFactor.cpp @@ -66,33 +66,6 @@ TEST( GaussianFactor, linearFactor ) EXPECT(assert_equal(expected,*lf)); } -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactor, keys ) -//{ -// // get the factor kf2 from the small linear factor graph -// Ordering ordering; ordering += kx1,kx2,kl1; -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// GaussianFactor::shared_ptr lf = fg[1]; -// list expected; -// expected.push_back(kx1); -// expected.push_back(kx2); -// EXPECT(lf->keys() == expected); -//} - -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactor, dimensions ) -//{ -// // get the factor kf2 from the small linear factor graph -// Ordering ordering; ordering += kx1,kx2,kl1; -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// -// // Check a single factor -// Dimensions expected; -// insert(expected)(kx1, 2)(kx2, 2); -// Dimensions actual = fg[1]->dimensions(); -// EXPECT(expected==actual); -//} - /* ************************************************************************* */ TEST( GaussianFactor, getDim ) { @@ -110,62 +83,6 @@ TEST( GaussianFactor, getDim ) EXPECT_LONGS_EQUAL(expected, actual); } -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactor, combine ) -//{ -// // create a small linear factor graph -// Ordering ordering; ordering += kx1,kx2,kl1; -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// -// // get two factors from it and insert the factors into a vector -// vector lfg; -// lfg.push_back(fg[4 - 1]); -// lfg.push_back(fg[2 - 1]); -// -// // combine in a factor -// GaussianFactor combined(lfg); -// -// // sigmas -// double sigma2 = 0.1; -// double sigma4 = 0.2; -// Vector sigmas = Vector_(4, sigma4, sigma4, sigma2, sigma2); -// -// // the expected combined linear factor -// Matrix Ax2 = Matrix_(4, 2, // x2 -// -5., 0., -// +0., -5., -// 10., 0., -// +0., 10.); -// -// Matrix Al1 = Matrix_(4, 2, // l1 -// 5., 0., -// 0., 5., -// 0., 0., -// 0., 0.); -// -// Matrix Ax1 = Matrix_(4, 2, // x1 -// 0.00, 0., // f4 -// 0.00, 0., // f4 -// -10., 0., // f2 -// 0.00, -10. // f2 -// ); -// -// // the RHS -// Vector b2(4); -// b2(0) = -1.0; -// b2(1) = 1.5; -// b2(2) = 2.0; -// b2(3) = -1.0; -// -// // use general constructor for making arbitrary factors -// vector > meas; -// meas.push_back(make_pair(kx2, Ax2)); -// meas.push_back(make_pair(kl1, Al1)); -// meas.push_back(make_pair(kx1, Ax1)); -// GaussianFactor expected(meas, b2, noiseModel::Diagonal::Sigmas(ones(4))); -// EXPECT(assert_equal(expected,combined)); -//} - /* ************************************************************************* */ TEST( GaussianFactor, error ) { @@ -186,47 +103,6 @@ TEST( GaussianFactor, error ) DOUBLES_EQUAL( 1.0, actual, 0.00000001 ); } -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactor, eliminate ) -//{ -// // create a small linear factor graph -// Ordering ordering; ordering += kx1,kx2,kl1; -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// -// // get two factors from it and insert the factors into a vector -// vector lfg; -// lfg.push_back(fg[4 - 1]); -// lfg.push_back(fg[2 - 1]); -// -// // combine in a factor -// GaussianFactor combined(lfg); -// -// // eliminate the combined factor -// GaussianConditional::shared_ptr actualCG; -// GaussianFactor::shared_ptr actualLF; -// boost::tie(actualCG,actualLF) = combined.eliminate(kx2); -// -// // create expected Conditional Gaussian -// Matrix I = eye(2)*sqrt(125.0); -// Matrix R11 = I, S12 = -0.2*I, S13 = -0.8*I; -// Vector d = I*Vector_(2,0.2,-0.14); -// -// // Check the conditional Gaussian -// GaussianConditional -// expectedCG(kx2, d, R11, kl1, S12, kx1, S13, repeat(2, 1.0)); -// -// // the expected linear factor -// I = eye(2)/0.2236; -// Matrix Bl1 = I, Bx1 = -I; -// Vector b1 = I*Vector_(2,0.0,0.2); -// -// GaussianFactor expectedLF(kl1, Bl1, kx1, Bx1, b1, repeat(2,1.0)); -// -// // check if the result matches -// EXPECT(assert_equal(expectedCG,*actualCG,1e-3)); -// EXPECT(assert_equal(expectedLF,*actualLF,1e-3)); -//} - /* ************************************************************************* */ TEST( GaussianFactor, matrix ) { @@ -328,66 +204,6 @@ void print(const list& i) { cout << endl; } -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactor, sparse ) -//{ -// // create a small linear factor graph -// Ordering ordering; ordering += kx1,kx2,kl1; -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// -// // get the factor kf2 from the factor graph -// GaussianFactor::shared_ptr lf = fg[1]; -// -// // render with a given ordering -// Ordering ord; -// ord += kx1,kx2; -// -// list i,j; -// list s; -// boost::tie(i,j,s) = lf->sparse(fg.columnIndices(ord)); -// -// list i1,j1; -// i1 += 1,2,1,2; -// j1 += 1,2,3,4; -// -// list s1; -// s1 += -10,-10,10,10; -// -// EXPECT(i==i1); -// EXPECT(j==j1); -// EXPECT(s==s1); -//} - -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactor, sparse2 ) -//{ -// // create a small linear factor graph -// Ordering ordering; ordering += kx1,kx2,kl1; -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// -// // get the factor kf2 from the factor graph -// GaussianFactor::shared_ptr lf = fg[1]; -// -// // render with a given ordering -// Ordering ord; -// ord += kx2,kl1,kx1; -// -// list i,j; -// list s; -// boost::tie(i,j,s) = lf->sparse(fg.columnIndices(ord)); -// -// list i1,j1; -// i1 += 1,2,1,2; -// j1 += 5,6,1,2; -// -// list s1; -// s1 += -10,-10,10,10; -// -// EXPECT(i==i1); -// EXPECT(j==j1); -// EXPECT(s==s1); -//} - /* ************************************************************************* */ TEST( GaussianFactor, size ) { diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index 7ec4e5317..ea91eda5a 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -47,7 +47,7 @@ static double sigmax1 = 0.786153, sigmax2 = 1.0/1.47292, sigmax3 = 0.671512, sig static const double tol = 1e-4; /* ************************************************************************* */ -TEST_UNSAFE( ISAM, iSAM_smoother ) +TEST( ISAM, iSAM_smoother ) { Ordering ordering; for (int t = 1; t <= 7; t++) ordering += X(t); @@ -76,31 +76,6 @@ TEST_UNSAFE( ISAM, iSAM_smoother ) EXPECT(assert_equal(e, optimized)); } -/* ************************************************************************* */ -// SL-FIX TEST( ISAM, iSAM_smoother2 ) -//{ -// // Create smoother with 7 nodes -// GaussianFactorGraph smoother = createSmoother(7); -// -// // Create initial tree from first 4 timestamps in reverse order ! -// Ordering ord; ord += X(4),X(3),X(2),X(1); -// GaussianFactorGraph factors1; -// for (int i=0;i<7;i++) factors1.push_back(smoother[i]); -// GaussianISAM actual(*inference::Eliminate(factors1)); -// -// // run iSAM with remaining factors -// GaussianFactorGraph factors2; -// for (int i=7;i<13;i++) factors2.push_back(smoother[i]); -// actual.update(factors2); -// -// // Create expected Bayes Tree by solving smoother with "natural" ordering -// Ordering ordering; -// for (int t = 1; t <= 7; t++) ordering += symbol('x', t); -// GaussianISAM expected(smoother.eliminate(ordering)); -// -// EXPECT(assert_equal(expected, actual)); -//} - /* ************************************************************************* * Bayes tree for smoother with "natural" ordering: C1 x6 x7 diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index 71a075ccb..14655a249 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -148,6 +148,65 @@ TEST( Graph, composePoses ) // LONGS_EQUAL(2, Ab2.size()); //} +///* ************************************************************************* */ +// SL-FIX TEST( FactorGraph, splitMinimumSpanningTree ) +//{ +// SymbolicFactorGraph G; +// G.push_factor("x1", "x2"); +// G.push_factor("x1", "x3"); +// G.push_factor("x1", "x4"); +// G.push_factor("x2", "x3"); +// G.push_factor("x2", "x4"); +// G.push_factor("x3", "x4"); +// +// SymbolicFactorGraph T, C; +// boost::tie(T, C) = G.splitMinimumSpanningTree(); +// +// SymbolicFactorGraph expectedT, expectedC; +// expectedT.push_factor("x1", "x2"); +// expectedT.push_factor("x1", "x3"); +// expectedT.push_factor("x1", "x4"); +// expectedC.push_factor("x2", "x3"); +// expectedC.push_factor("x2", "x4"); +// expectedC.push_factor("x3", "x4"); +// CHECK(assert_equal(expectedT,T)); +// CHECK(assert_equal(expectedC,C)); +//} + +///* ************************************************************************* */ +///** +// * x1 - x2 - x3 - x4 - x5 +// * | | / | +// * l1 l2 l3 +// */ +// SL-FIX TEST( FactorGraph, removeSingletons ) +//{ +// SymbolicFactorGraph G; +// G.push_factor("x1", "x2"); +// G.push_factor("x2", "x3"); +// G.push_factor("x3", "x4"); +// G.push_factor("x4", "x5"); +// G.push_factor("x2", "l1"); +// G.push_factor("x3", "l2"); +// G.push_factor("x4", "l2"); +// G.push_factor("x4", "l3"); +// +// SymbolicFactorGraph singletonGraph; +// set singletons; +// boost::tie(singletonGraph, singletons) = G.removeSingletons(); +// +// set singletons_excepted; singletons_excepted += "x1", "x2", "x5", "l1", "l3"; +// CHECK(singletons_excepted == singletons); +// +// SymbolicFactorGraph singletonGraph_excepted; +// singletonGraph_excepted.push_factor("x2", "l1"); +// singletonGraph_excepted.push_factor("x4", "l3"); +// singletonGraph_excepted.push_factor("x1", "x2"); +// singletonGraph_excepted.push_factor("x4", "x5"); +// singletonGraph_excepted.push_factor("x2", "x3"); +// CHECK(singletonGraph_excepted.equals(singletonGraph)); +//} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index b1717a79b..c8824749d 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -73,8 +73,8 @@ TEST( NonlinearFactor, equals2 ) Graph::sharedFactor f0 = fg[0], f1 = fg[1]; CHECK(f0->equals(*f0)); -// SL-FIX CHECK(!f0->equals(*f1)); -// SL-FIX CHECK(!f1->equals(*f0)); + CHECK(!f0->equals(*f1)); + CHECK(!f1->equals(*f0)); } /* ************************************************************************* */