diff --git a/gtsam/linear/tests/testJacobianFactorGraph.cpp b/gtsam/linear/tests/testJacobianFactorGraph.cpp new file mode 100644 index 000000000..e756d83e7 --- /dev/null +++ b/gtsam/linear/tests/testJacobianFactorGraph.cpp @@ -0,0 +1,92 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testJacobianFactorGraph.cpp + * @brief Unit tests for JacobianFactorGraph + * @author Yong Dian Jian + **/ + +#include +#include + + +///* ************************************************************************* */ +//TEST( GaussianFactorGraph, gradient ) +//{ +// GaussianFactorGraph fg = createGaussianFactorGraph(); +// +// // Construct expected gradient +// VectorValues expected; +// +// // 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2 +// // worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5] +// expected.insert(L(1),Vector_(2, 5.0,-12.5)); +// expected.insert(X(1),Vector_(2, 30.0, 5.0)); +// expected.insert(X(2),Vector_(2,-25.0, 17.5)); +// +// // Check the gradient at delta=0 +// VectorValues zero = createZeroDelta(); +// VectorValues actual = fg.gradient(zero); +// EXPECT(assert_equal(expected,actual)); +// +// // Check it numerically for good measure +// Vector numerical_g = numericalGradient(error,zero,0.001); +// EXPECT(assert_equal(Vector_(6,5.0,-12.5,30.0,5.0,-25.0,17.5),numerical_g)); +// +// // Check the gradient at the solution (should be zero) +// Ordering ord; +// ord += X(2),L(1),X(1); +// GaussianFactorGraph fg2 = createGaussianFactorGraph(); +// VectorValues solution = fg2.optimize(ord); // destructive +// VectorValues actual2 = fg.gradient(solution); +// EXPECT(assert_equal(zero,actual2)); +//} + +///* ************************************************************************* */ +//TEST( GaussianFactorGraph, transposeMultiplication ) +//{ +// // create an ordering +// Ordering ord; ord += X(2),L(1),X(1); +// +// GaussianFactorGraph A = createGaussianFactorGraph(ord); +// Errors e; +// e += Vector_(2, 0.0, 0.0); +// e += Vector_(2,15.0, 0.0); +// e += Vector_(2, 0.0,-5.0); +// e += Vector_(2,-7.5,-5.0); +// +// VectorValues expected = createZeroDelta(ord), actual = A ^ e; +// expected[ord[L(1)]] = Vector_(2, -37.5,-50.0); +// expected[ord[X(1)]] = Vector_(2,-150.0, 25.0); +// expected[ord[X(2)]] = Vector_(2, 187.5, 25.0); +// EXPECT(assert_equal(expected,actual)); +//} + +///* ************************************************************************* */ +//TEST( GaussianFactorGraph, rhs ) +//{ +// // create an ordering +// Ordering ord; ord += X(2),L(1),X(1); +// +// GaussianFactorGraph Ab = createGaussianFactorGraph(ord); +// Errors expected = createZeroDelta(ord), actual = Ab.rhs(); +// expected.push_back(Vector_(2,-1.0,-1.0)); +// expected.push_back(Vector_(2, 2.0,-1.0)); +// expected.push_back(Vector_(2, 0.0, 1.0)); +// expected.push_back(Vector_(2,-1.0, 1.5)); +// EXPECT(assert_equal(expected,actual)); +//} + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 20972add9..c6ec69d4e 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -56,143 +56,17 @@ TEST( GaussianFactorGraph, equals ) { } /* ************************************************************************* */ -//TEST( GaussianFactorGraph, error ) { -// Ordering ordering; ordering += X(1),X(2),L(1); -// FactorGraph fg = createGaussianFactorGraph(ordering); -// VectorValues cfg = createZeroDelta(ordering); -// -// // note the error is the same as in testNonlinearFactorGraph as a -// // zero delta config in the linear graph is equivalent to noisy in -// // non-linear, which is really linear under the hood -// double actual = fg.error(cfg); -// DOUBLES_EQUAL( 5.625, actual, 1e-9 ); -//} +TEST( GaussianFactorGraph, error ) { + Ordering ordering; ordering += X(1),X(2),L(1); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + VectorValues cfg = createZeroDelta(ordering); -/* ************************************************************************* */ -/* unit test for find seperator */ -/* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, find_separator ) -//{ -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// -// set separator = fg.find_separator(X(2)); -// set expected; -// expected.insert(X(1)); -// expected.insert(L(1)); -// -// EXPECT(separator.size()==expected.size()); -// set::iterator it1 = separator.begin(), it2 = expected.begin(); -// for(; it1!=separator.end(); it1++, it2++) -// EXPECT(*it1 == *it2); -//} - -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactorGraph, combine_factors_x1 ) -//{ -// // create a small example for a linear factor graph -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// -// // combine all factors -// GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,X(1)); -// -// // the expected linear factor -// Matrix Al1 = Matrix_(6,2, -// 0., 0., -// 0., 0., -// 0., 0., -// 0., 0., -// 5., 0., -// 0., 5. -// ); -// -// Matrix Ax1 = Matrix_(6,2, -// 10., 0., -// 0., 10., -// -10., 0., -// 0.,-10., -// -5., 0., -// 0.,-5. -// ); -// -// Matrix Ax2 = Matrix_(6,2, -// 0., 0., -// 0., 0., -// 10., 0., -// 0., 10., -// 0., 0., -// 0., 0. -// ); -// -// // the expected RHS vector -// Vector b(6); -// b(0) = -1; -// b(1) = -1; -// b(2) = 2; -// b(3) = -1; -// b(4) = 0; -// b(5) = 1; -// -// vector > meas; -// meas.push_back(make_pair(L(1), Al1)); -// meas.push_back(make_pair(X(1), Ax1)); -// meas.push_back(make_pair(X(2), Ax2)); -// GaussianFactor expected(meas, b, ones(6)); -// //GaussianFactor expected(L(1), Al1, X(1), Ax1, X(2), Ax2, b); -// -// // check if the two factors are the same -// EXPECT(assert_equal(expected,*actual)); -//} -// -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactorGraph, combine_factors_x2 ) -//{ -// // create a small example for a linear factor graph -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// -// // combine all factors -// GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,X(2)); -// -// // the expected linear factor -// Matrix Al1 = Matrix_(4,2, -// // l1 -// 0., 0., -// 0., 0., -// 5., 0., -// 0., 5. -// ); -// -// Matrix Ax1 = Matrix_(4,2, -// // x1 -// -10., 0., // f2 -// 0.,-10., // f2 -// 0., 0., // f4 -// 0., 0. // f4 -// ); -// -// Matrix Ax2 = Matrix_(4,2, -// // x2 -// 10., 0., -// 0., 10., -// -5., 0., -// 0.,-5. -// ); -// -// // the expected RHS vector -// Vector b(4); -// b(0) = 2; -// b(1) = -1; -// b(2) = -1; -// b(3) = 1.5; -// -// vector > meas; -// meas.push_back(make_pair(L(1), Al1)); -// meas.push_back(make_pair(X(1), Ax1)); -// meas.push_back(make_pair(X(2), Ax2)); -// GaussianFactor expected(meas, b, ones(4)); -// -// // check if the two factors are the same -// EXPECT(assert_equal(expected,*actual)); -//} + // note the error is the same as in testNonlinearFactorGraph as a + // zero delta config in the linear graph is equivalent to noisy in + // non-linear, which is really linear under the hood + double actual = fg.error(cfg); + DOUBLES_EQUAL( 5.625, actual, 1e-9 ); +} /* ************************************************************************* */ TEST( GaussianFactorGraph, eliminateOne_x1 ) @@ -336,47 +210,6 @@ TEST( GaussianFactorGraph, eliminateAll ) EXPECT(assert_equal(expected,actualQR,tol)); } -///* ************************************************************************* */ -//TEST( GaussianFactorGraph, eliminateAll_fast ) -//{ -// // create expected Chordal bayes Net -// Matrix I = eye(2); -// -// Vector d1 = Vector_(2, -0.1,-0.1); -// GaussianBayesNet expected = simpleGaussian(X(1),d1,0.1); -// -// double sig1 = 0.149071; -// Vector d2 = Vector_(2, 0.0, 0.2)/sig1, sigma2 = ones(2); -// push_front(expected,L(1),d2, I/sig1,X(1), (-1)*I/sig1,sigma2); -// -// double sig2 = 0.0894427; -// Vector d3 = Vector_(2, 0.2, -0.14)/sig2, sigma3 = ones(2); -// push_front(expected,X(2),d3, I/sig2,L(1), (-0.2)*I/sig2, X(1), (-0.8)*I/sig2, sigma3); -// -// // Check one ordering -// GaussianFactorGraph fg1 = createGaussianFactorGraph(); -// Ordering ordering; -// ordering += X(2),L(1),X(1); -// GaussianBayesNet actual = fg1.eliminate(ordering, false); -// EXPECT(assert_equal(expected,actual,tol)); -//} - -///* ************************************************************************* */ -//TEST( GaussianFactorGraph, add_priors ) -//{ -// Ordering ordering; ordering += L(1),X(1),X(2); -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// GaussianFactorGraph actual = fg.add_priors(3, vector(3,2)); -// GaussianFactorGraph expected = createGaussianFactorGraph(ordering); -// Matrix A = eye(2); -// Vector b = zero(2); -// SharedDiagonal sigma = noiseModel::Isotropic::Sigma(2,3.0); -// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[L(1)],A,b,sigma))); -// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[X(1)],A,b,sigma))); -// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[X(2)],A,b,sigma))); -// EXPECT(assert_equal(expected,actual)); -//} - /* ************************************************************************* */ TEST( GaussianFactorGraph, copying ) { @@ -397,46 +230,6 @@ TEST( GaussianFactorGraph, copying ) EXPECT(assert_equal(expected,actual)); } -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactorGraph, matrix ) -//{ -// // render with a given ordering -// Ordering ord; -// ord += X(2),L(1),X(1); -// -// // Create a graph -// GaussianFactorGraph fg = createGaussianFactorGraph(ordering); -// -// Matrix A; Vector b; -// boost::tie(A,b) = fg.matrix(); -// -// Matrix A1 = Matrix_(2*4,3*2, -// +0., 0., 0., 0., 10., 0., // unary factor on x1 (prior) -// +0., 0., 0., 0., 0., 10., -// 10., 0., 0., 0.,-10., 0., // binary factor on x2,x1 (odometry) -// +0., 10., 0., 0., 0.,-10., -// +0., 0., 5., 0., -5., 0., // binary factor on l1,x1 (z1) -// +0., 0., 0., 5., 0., -5., -// -5., 0., 5., 0., 0., 0., // binary factor on x2,l1 (z2) -// +0., -5., 0., 5., 0., 0. -// ); -// Vector b1 = Vector_(8,-1., -1., 2., -1., 0., 1., -1., 1.5); -// -// EQUALITY(A,A1); -// EXPECT(b==b1); -//} - -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactorGraph, sizeOfA ) -//{ -// // create a small linear factor graph -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// -// pair mn = fg.sizeOfA(); -// EXPECT(8 == mn.first); -// EXPECT(6 == mn.second); -//} - /* ************************************************************************* */ TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet ) { @@ -451,11 +244,6 @@ TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet ) GaussianFactorGraph fg2(CBN); GaussianBayesNet CBN2 = *GaussianSequentialSolver(fg2).eliminate(); EXPECT(assert_equal(CBN,CBN2)); - - // Base FactorGraph only -// FactorGraph fg3(CBN); -// GaussianBayesNet CBN3 = gtsam::eliminate(fg3,ord); -// EXPECT(assert_equal(CBN,CBN3)); } /* ************************************************************************* */ @@ -469,16 +257,6 @@ TEST( GaussianFactorGraph, getOrdering) EXPECT(assert_equal(expected,actual)); } -// SL-FIX TEST( GaussianFactorGraph, getOrdering2) -//{ -// Ordering expected; -// expected += L(1),X(1); -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// set interested; interested += L(1),X(1); -// Ordering actual = fg.getOrdering(interested); -// EXPECT(assert_equal(expected,actual)); -//} - /* ************************************************************************* */ TEST( GaussianFactorGraph, optimize_Cholesky ) { @@ -515,24 +293,6 @@ TEST( GaussianFactorGraph, optimize_QR ) EXPECT(assert_equal(expected,actual)); } -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactorGraph, optimizeMultiFrontlas ) -//{ -// // create an ordering -// Ordering ord; ord += X(2),L(1),X(1); -// -// // create a graph -// GaussianFactorGraph fg = createGaussianFactorGraph(ord); -// -// // optimize the graph -// VectorValues actual = fg.optimizeMultiFrontals(ord); -// -// // verify -// VectorValues expected = createCorrectDelta(); -// -// EXPECT(assert_equal(expected,actual)); -//} - /* ************************************************************************* */ TEST( GaussianFactorGraph, combine) { @@ -585,48 +345,6 @@ void print(vector v) { cout << endl; } -///* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, factor_lookup) -//{ -// // create a test graph -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// -// // ask for all factor indices connected to x1 -// list x1_factors = fg.factors(X(1)); -// size_t x1_indices[] = { 0, 1, 2 }; -// list x1_expected(x1_indices, x1_indices + 3); -// EXPECT(x1_factors==x1_expected); -// -// // ask for all factor indices connected to x2 -// list x2_factors = fg.factors(X(2)); -// size_t x2_indices[] = { 1, 3 }; -// list x2_expected(x2_indices, x2_indices + 2); -// EXPECT(x2_factors==x2_expected); -//} - -///* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, findAndRemoveFactors ) -//{ -// // create the graph -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// -// // We expect to remove these three factors: 0, 1, 2 -// GaussianFactor::shared_ptr f0 = fg[0]; -// GaussianFactor::shared_ptr f1 = fg[1]; -// GaussianFactor::shared_ptr f2 = fg[2]; -// -// // call the function -// vector factors = fg.findAndRemoveFactors(X(1)); -// -// // Check the factors -// EXPECT(f0==factors[0]); -// EXPECT(f1==factors[1]); -// EXPECT(f2==factors[2]); -// -// // EXPECT if the factors are deleted from the factor graph -// LONGS_EQUAL(1,fg.nrFactors()); -//} - /* ************************************************************************* */ TEST(GaussianFactorGraph, createSmoother) { @@ -636,35 +354,6 @@ TEST(GaussianFactorGraph, createSmoother) LONGS_EQUAL(5,fg2.size()); } -///* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, variables ) -//{ -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// Dimensions expected; -// insert(expected)(L(1), 2)(X(1), 2)(X(2), 2); -// Dimensions actual = fg.dimensions(); -// EXPECT(expected==actual); -//} - -///* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, keys ) -//{ -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// Ordering expected; -// expected += L(1),X(1),X(2); -// EXPECT(assert_equal(expected,fg.keys())); -//} - -///* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, involves ) -//{ -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// EXPECT(fg.involves(L(1))); -// EXPECT(fg.involves(X(1))); -// EXPECT(fg.involves(X(2))); -// EXPECT(!fg.involves(X(3))); -//} - /* ************************************************************************* */ double error(const VectorValues& x) { // create an ordering @@ -674,38 +363,6 @@ double error(const VectorValues& x) { return fg.error(x); } -///* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, gradient ) -//{ -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// -// // Construct expected gradient -// VectorValues expected; -// -// // 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2 -// // worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5] -// expected.insert(L(1),Vector_(2, 5.0,-12.5)); -// expected.insert(X(1),Vector_(2, 30.0, 5.0)); -// expected.insert(X(2),Vector_(2,-25.0, 17.5)); -// -// // Check the gradient at delta=0 -// VectorValues zero = createZeroDelta(); -// VectorValues actual = fg.gradient(zero); -// EXPECT(assert_equal(expected,actual)); -// -// // Check it numerically for good measure -// Vector numerical_g = numericalGradient(error,zero,0.001); -// EXPECT(assert_equal(Vector_(6,5.0,-12.5,30.0,5.0,-25.0,17.5),numerical_g)); -// -// // Check the gradient at the solution (should be zero) -// Ordering ord; -// ord += X(2),L(1),X(1); -// GaussianFactorGraph fg2 = createGaussianFactorGraph(); -// VectorValues solution = fg2.optimize(ord); // destructive -// VectorValues actual2 = fg.gradient(solution); -// EXPECT(assert_equal(zero,actual2)); -//} - /* ************************************************************************* */ TEST( GaussianFactorGraph, multiplication ) { @@ -723,41 +380,6 @@ TEST( GaussianFactorGraph, multiplication ) EXPECT(assert_equal(expected,actual)); } -///* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, transposeMultiplication ) -//{ -// // create an ordering -// Ordering ord; ord += X(2),L(1),X(1); -// -// GaussianFactorGraph A = createGaussianFactorGraph(ord); -// Errors e; -// e += Vector_(2, 0.0, 0.0); -// e += Vector_(2,15.0, 0.0); -// e += Vector_(2, 0.0,-5.0); -// e += Vector_(2,-7.5,-5.0); -// -// VectorValues expected = createZeroDelta(ord), actual = A ^ e; -// expected[ord[L(1)]] = Vector_(2, -37.5,-50.0); -// expected[ord[X(1)]] = Vector_(2,-150.0, 25.0); -// expected[ord[X(2)]] = Vector_(2, 187.5, 25.0); -// EXPECT(assert_equal(expected,actual)); -//} - -///* ************************************************************************* */ -// SL-NEEDED? TEST( GaussianFactorGraph, rhs ) -//{ -// // create an ordering -// Ordering ord; ord += X(2),L(1),X(1); -// -// GaussianFactorGraph Ab = createGaussianFactorGraph(ord); -// Errors expected = createZeroDelta(ord), actual = Ab.rhs(); -// expected.push_back(Vector_(2,-1.0,-1.0)); -// expected.push_back(Vector_(2, 2.0,-1.0)); -// expected.push_back(Vector_(2, 0.0, 1.0)); -// expected.push_back(Vector_(2,-1.0, 1.5)); -// EXPECT(assert_equal(expected,actual)); -//} - /* ************************************************************************* */ // Extra test on elimination prompted by Michael's email to Frank 1/4/2010 TEST( GaussianFactorGraph, elimination ) @@ -824,22 +446,6 @@ TEST( GaussianFactorGraph, constrained_single ) EXPECT(assert_equal(expected, actual)); } -///* ************************************************************************* */ -//SL-FIX TEST( GaussianFactorGraph, constrained_single2 ) -//{ -// // get a graph with a constraint in it -// GaussianFactorGraph fg = createSingleConstraintGraph(); -// -// // eliminate and solve -// Ordering ord; -// ord += "yk, x"; -// VectorValues actual = fg.optimize(ord); -// -// // verify -// VectorValues expected = createSingleConstraintValues(); -// EXPECT(assert_equal(expected, actual)); -//} - /* ************************************************************************* */ TEST( GaussianFactorGraph, constrained_multi1 ) { @@ -855,68 +461,9 @@ TEST( GaussianFactorGraph, constrained_multi1 ) EXPECT(assert_equal(expected, actual)); } -///* ************************************************************************* */ -//SL-FIX TEST( GaussianFactorGraph, constrained_multi2 ) -//{ -// // get a graph with a constraint in it -// GaussianFactorGraph fg = createMultiConstraintGraph(); -// -// // eliminate and solve -// Ordering ord; -// ord += "zk, xk, y"; -// VectorValues actual = fg.optimize(ord); -// -// // verify -// VectorValues expected = createMultiConstraintValues(); -// EXPECT(assert_equal(expected, actual)); -//} - /* ************************************************************************* */ -SharedDiagonal model = noiseModel::Isotropic::Sigma(2,1); - -// SL-FIX TEST( GaussianFactorGraph, findMinimumSpanningTree ) -//{ -// GaussianFactorGraph g; -// Matrix I = eye(2); -// Vector b = Vector_(0, 0, 0); -// g.add(X(1), I, X(2), I, b, model); -// g.add(X(1), I, X(3), I, b, model); -// g.add(X(1), I, X(4), I, b, model); -// g.add(X(2), I, X(3), I, b, model); -// g.add(X(2), I, X(4), I, b, model); -// g.add(X(3), I, X(4), I, b, model); -// -// map tree = g.findMinimumSpanningTree(); -// EXPECT(tree[X(1)].compare(X(1))==0); -// EXPECT(tree[X(2)].compare(X(1))==0); -// EXPECT(tree[X(3)].compare(X(1))==0); -// EXPECT(tree[X(4)].compare(X(1))==0); -//} - -///* ************************************************************************* */ -// SL-FIX TEST( GaussianFactorGraph, split ) -//{ -// GaussianFactorGraph g; -// Matrix I = eye(2); -// Vector b = Vector_(0, 0, 0); -// g.add(X(1), I, X(2), I, b, model); -// g.add(X(1), I, X(3), I, b, model); -// g.add(X(1), I, X(4), I, b, model); -// g.add(X(2), I, X(3), I, b, model); -// g.add(X(2), I, X(4), I, b, model); -// -// PredecessorMap tree; -// tree[X(1)] = X(1); -// tree[X(2)] = X(1); -// tree[X(3)] = X(1); -// tree[X(4)] = X(1); -// -// GaussianFactorGraph Ab1, Ab2; -// g.split(tree, Ab1, Ab2); -// LONGS_EQUAL(3, Ab1.size()); -// LONGS_EQUAL(2, Ab2.size()); -//} +static SharedDiagonal model = noiseModel::Isotropic::Sigma(2,1); /* ************************************************************************* */ TEST(GaussianFactorGraph, replace) @@ -935,91 +482,18 @@ TEST(GaussianFactorGraph, replace) GaussianFactorGraph actual; actual.push_back(f1); -// actual.checkGraphConsistency(); actual.push_back(f2); -// actual.checkGraphConsistency(); actual.push_back(f3); -// actual.checkGraphConsistency(); actual.replace(0, f4); -// actual.checkGraphConsistency(); GaussianFactorGraph expected; expected.push_back(f4); -// actual.checkGraphConsistency(); expected.push_back(f2); -// actual.checkGraphConsistency(); expected.push_back(f3); -// actual.checkGraphConsistency(); EXPECT(assert_equal(expected, actual)); } -///* ************************************************************************* */ -//TEST ( GaussianFactorGraph, combine_matrix ) { -// // create a small linear factor graph -// GaussianFactorGraph fg = createGaussianFactorGraph(); -// Dimensions dimensions = fg.dimensions(); -// -// // 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 -// Matrix Ab; SharedDiagonal noise; -// Ordering order; order += X(2), L(1), X(1); -// boost::tie(Ab, noise) = combineFactorsAndCreateMatrix(lfg, order, dimensions); -// -// // the expected augmented matrix -// Matrix expAb = Matrix_(4, 7, -// -5., 0., 5., 0., 0., 0.,-1.0, -// +0., -5., 0., 5., 0., 0., 1.5, -// 10., 0., 0., 0.,-10., 0., 2.0, -// +0., 10., 0., 0., 0.,-10.,-1.0); -// -// // expected noise model -// SharedDiagonal expModel = noiseModel::Unit::Create(4); -// -// EXPECT(assert_equal(expAb, Ab)); -// EXPECT(assert_equal(*expModel, *noise)); -//} - -/* ************************************************************************* */ -/* - * x2 x1 x3 b - * 1 1 1 1 1 0 1 - * 1 1 1 -> 1 1 1 - * 1 1 1 1 - */ -// SL-NEEDED? TEST ( GaussianFactorGraph, eliminateFrontals ) { -// typedef GaussianFactorGraph::sharedFactor Factor; -// SharedDiagonal model(Vector_(1, 0.5)); -// GaussianFactorGraph fg; -// Factor factor1(new JacobianFactor(X(1), Matrix_(1,1,1.), X(2), Matrix_(1,1,1.), Vector_(1,1.), model)); -// Factor factor2(new JacobianFactor(X(2), Matrix_(1,1,1.), X(3), Matrix_(1,1,1.), Vector_(1,1.), model)); -// Factor factor3(new JacobianFactor(X(3), Matrix_(1,1,1.), X(3), Matrix_(1,1,1.), Vector_(1,1.), model)); -// fg.push_back(factor1); -// fg.push_back(factor2); -// fg.push_back(factor3); -// -// Ordering frontals; frontals += X(2), X(1); -// GaussianBayesNet bn = fg.eliminateFrontals(frontals); -// -// GaussianBayesNet bn_expected; -// GaussianBayesNet::sharedConditional conditional1(new GaussianConditional(X(2), Vector_(1, 2.), Matrix_(1, 1, 2.), -// X(1), Matrix_(1, 1, 1.), X(3), Matrix_(1, 1, 1.), Vector_(1, 1.))); -// GaussianBayesNet::sharedConditional conditional2(new GaussianConditional(X(1), Vector_(1, 0.), Matrix_(1, 1, -1.), -// X(3), Matrix_(1, 1, 1.), Vector_(1, 1.))); -// bn_expected.push_back(conditional1); -// bn_expected.push_back(conditional2); -// EXPECT(assert_equal(bn_expected, bn)); -// -// GaussianFactorGraph::sharedFactor factor_expected(new JacobianFactor(X(3), Matrix_(1, 1, 2.), Vector_(1, 2.), SharedDiagonal(Vector_(1, 1.)))); -// GaussianFactorGraph fg_expected; -// fg_expected.push_back(factor_expected); -// EXPECT(assert_equal(fg_expected, fg)); -//} - /* ************************************************************************* */ TEST(GaussianFactorGraph, createSmoother2) { diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index 93ecd6dbe..71a075ccb 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -105,6 +105,49 @@ TEST( Graph, composePoses ) CHECK(assert_equal(expected, *actual)); } +// SL-FIX TEST( GaussianFactorGraph, findMinimumSpanningTree ) +//{ +// GaussianFactorGraph g; +// Matrix I = eye(2); +// Vector b = Vector_(0, 0, 0); +// g.add(X(1), I, X(2), I, b, model); +// g.add(X(1), I, X(3), I, b, model); +// g.add(X(1), I, X(4), I, b, model); +// g.add(X(2), I, X(3), I, b, model); +// g.add(X(2), I, X(4), I, b, model); +// g.add(X(3), I, X(4), I, b, model); +// +// map tree = g.findMinimumSpanningTree(); +// EXPECT(tree[X(1)].compare(X(1))==0); +// EXPECT(tree[X(2)].compare(X(1))==0); +// EXPECT(tree[X(3)].compare(X(1))==0); +// EXPECT(tree[X(4)].compare(X(1))==0); +//} + +///* ************************************************************************* */ +// SL-FIX TEST( GaussianFactorGraph, split ) +//{ +// GaussianFactorGraph g; +// Matrix I = eye(2); +// Vector b = Vector_(0, 0, 0); +// g.add(X(1), I, X(2), I, b, model); +// g.add(X(1), I, X(3), I, b, model); +// g.add(X(1), I, X(4), I, b, model); +// g.add(X(2), I, X(3), I, b, model); +// g.add(X(2), I, X(4), I, b, model); +// +// PredecessorMap tree; +// tree[X(1)] = X(1); +// tree[X(2)] = X(1); +// tree[X(3)] = X(1); +// tree[X(4)] = X(1); +// +// GaussianFactorGraph Ab1, Ab2; +// g.split(tree, Ab1, Ab2); +// LONGS_EQUAL(3, Ab1.size()); +// LONGS_EQUAL(2, Ab2.size()); +//} + /* ************************************************************************* */ int main() { TestResult tr;