From 2d60d2cdfa673351a824e38b36e28ecfcb417fbd Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 6 Aug 2013 18:04:40 +0000 Subject: [PATCH] Fixed more unit tests --- gtsam/base/tests/testNumericalDerivative.cpp | 8 ++--- gtsam/slam/tests/testReferenceFrameFactor.cpp | 2 +- .../tests/testBatchFixedLagSmoother.cpp | 2 +- .../tests/testConcurrentBatchFilter.cpp | 2 +- .../tests/testConcurrentBatchSmoother.cpp | 2 +- .../tests/testIncrementalFixedLagSmoother.cpp | 2 +- tests/testExtendedKalmanFilter.cpp | 4 +-- tests/testGaussianBayesTree.cpp | 2 -- tests/testGaussianISAM2.cpp | 18 +++++------ tests/testNonlinearFactor.cpp | 30 +++++++++---------- tests/testOccupancyGrid.cpp | 2 +- wrap/tests/testWrap.cpp | 2 +- 12 files changed, 37 insertions(+), 39 deletions(-) diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp index 9d272d682..5647a0283 100644 --- a/gtsam/base/tests/testNumericalDerivative.cpp +++ b/gtsam/base/tests/testNumericalDerivative.cpp @@ -28,7 +28,7 @@ double f(const LieVector& x) { } /* ************************************************************************* */ -TEST_UNSAFE(testNumericalDerivative, numericalHessian) { +TEST(testNumericalDerivative, numericalHessian) { LieVector center = ones(2); Matrix expected = Matrix_(2,2, @@ -47,7 +47,7 @@ double f2(const LieVector& x) { } /* ************************************************************************* */ -TEST_UNSAFE(testNumericalDerivative, numericalHessian2) { +TEST(testNumericalDerivative, numericalHessian2) { LieVector center(2, 0.5, 1.0); Matrix expected = Matrix_(2,2, @@ -66,7 +66,7 @@ double f3(const LieVector& x1, const LieVector& x2) { } /* ************************************************************************* */ -TEST_UNSAFE(testNumericalDerivative, numericalHessian211) { +TEST(testNumericalDerivative, numericalHessian211) { LieVector center1(1, 1.0), center2(1, 5.0); Matrix expected11 = Matrix_(1,1,-sin(center1(0))*cos(center2(0))); @@ -89,7 +89,7 @@ double f4(const LieVector& x, const LieVector& y, const LieVector& z) { } /* ************************************************************************* */ -TEST_UNSAFE(testNumericalDerivative, numericalHessian311) { +TEST(testNumericalDerivative, numericalHessian311) { LieVector center1(1, 1.0), center2(1, 2.0), center3(1, 3.0); double x = center1(0), y = center2(0), z = center3(0); Matrix expected11 = Matrix_(1,1,-sin(x)*cos(y)*z*z); diff --git a/gtsam/slam/tests/testReferenceFrameFactor.cpp b/gtsam/slam/tests/testReferenceFrameFactor.cpp index 815ce391a..d84c5b41f 100644 --- a/gtsam/slam/tests/testReferenceFrameFactor.cpp +++ b/gtsam/slam/tests/testReferenceFrameFactor.cpp @@ -116,7 +116,7 @@ TEST( ReferenceFrameFactor, jacobians_zero ) { } /* ************************************************************************* */ -TEST_UNSAFE( ReferenceFrameFactor, converge_trans ) { +TEST( ReferenceFrameFactor, converge_trans ) { // initial points Point2 local1(2.0, 2.0), local2(4.0, 5.0), diff --git a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp index f4b326719..8728092e6 100644 --- a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp @@ -46,7 +46,7 @@ bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullini } /* ************************************************************************* */ -TEST_UNSAFE( BatchFixedLagSmoother, Example ) +TEST( BatchFixedLagSmoother, Example ) { // Test the BatchFixedLagSmoother in a pure linear environment. Thus, full optimization and // the BatchFixedLagSmoother should be identical (even with the linearized approximations at diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp index f4f4190e9..bd27c09c9 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -598,7 +598,7 @@ TEST( ConcurrentBatchFilter, update_incremental_with_marginalization ) } /* ************************************************************************* */ -TEST_UNSAFE( ConcurrentBatchFilter, synchronize ) +TEST( ConcurrentBatchFilter, synchronize ) { // Test the 'synchronize' function of the ConcurrentBatchFilter in a nonlinear environment. // The filter is operating on a known tree structure, so the factors and summarization can diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp index 04b6f4788..0e89bc589 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -473,7 +473,7 @@ TEST( ConcurrentBatchSmoother, update_incremental ) } /* ************************************************************************* */ -TEST_UNSAFE( ConcurrentBatchSmoother, synchronize ) +TEST( ConcurrentBatchSmoother, synchronize ) { // Test the 'synchronize' function of the ConcurrentBatchSmoother in a nonlinear environment. // The smoother is operating on a known tree structure, so the factors and summarization can diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index 0a4a1fa74..a2941acaa 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -50,7 +50,7 @@ bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullini } /* ************************************************************************* */ -TEST_UNSAFE( IncrementalFixedLagSmoother, Example ) +TEST( IncrementalFixedLagSmoother, Example ) { // Test the IncrementalFixedLagSmoother in a pure linear environment. Thus, full optimization and // the IncrementalFixedLagSmoother should be identical (even with the linearized approximations at diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index 24ca2d6e5..ac3705cab 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -30,7 +30,7 @@ using symbol_shorthand::X; using symbol_shorthand::L; /* ************************************************************************* */ -TEST_UNSAFE( ExtendedKalmanFilter, linear ) { +TEST( ExtendedKalmanFilter, linear ) { // Create the TestKeys for our example Symbol x0('x',0), x1('x',1), x2('x',2), x3('x',3); @@ -364,7 +364,7 @@ public: /* ************************************************************************* */ -TEST_UNSAFE( ExtendedKalmanFilter, nonlinear ) { +TEST( ExtendedKalmanFilter, nonlinear ) { // Create the set of expected output TestValues (generated using Matlab Kalman Filter) Point2 expected_predict[10]; diff --git a/tests/testGaussianBayesTree.cpp b/tests/testGaussianBayesTree.cpp index 0f77b3dd1..653ca0809 100644 --- a/tests/testGaussianBayesTree.cpp +++ b/tests/testGaussianBayesTree.cpp @@ -37,8 +37,6 @@ using namespace example; using symbol_shorthand::X; using symbol_shorthand::L; -#define TEST TEST_UNSAFE - /* ************************************************************************* */ // Some numbers that should be consistent among all smoother tests diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 5fed6a42a..9a3c84bb8 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -143,7 +143,7 @@ ISAM2 createSlamlikeISAM2( } /* ************************************************************************* */ -TEST_UNSAFE(ISAM2, ImplAddVariables) { +TEST(ISAM2, ImplAddVariables) { // Create initial state Values theta; @@ -211,7 +211,7 @@ TEST_UNSAFE(ISAM2, ImplAddVariables) { } /* ************************************************************************* */ -TEST_UNSAFE(ISAM2, ImplRemoveVariables) { +TEST(ISAM2, ImplRemoveVariables) { // Create initial state Values theta; @@ -610,7 +610,7 @@ TEST(ISAM2, removeFactors) } /* ************************************************************************* */ -TEST_UNSAFE(ISAM2, removeVariables) +TEST(ISAM2, removeVariables) { // These variables will be reused and accumulate factors and values Values fullinit; @@ -633,7 +633,7 @@ TEST_UNSAFE(ISAM2, removeVariables) } /* ************************************************************************* */ -TEST_UNSAFE(ISAM2, swapFactors) +TEST(ISAM2, swapFactors) { // This test builds a graph in the same way as the "slamlike" test above, but // then swaps the 2nd-to-last landmark measurement with a different one @@ -888,7 +888,7 @@ namespace { } /* ************************************************************************* */ -TEST_UNSAFE(ISAM2, marginalizeLeaves1) +TEST(ISAM2, marginalizeLeaves1) { ISAM2 isam; @@ -917,7 +917,7 @@ TEST_UNSAFE(ISAM2, marginalizeLeaves1) } /* ************************************************************************* */ -TEST_UNSAFE(ISAM2, marginalizeLeaves2) +TEST(ISAM2, marginalizeLeaves2) { ISAM2 isam; @@ -949,7 +949,7 @@ TEST_UNSAFE(ISAM2, marginalizeLeaves2) } /* ************************************************************************* */ -TEST_UNSAFE(ISAM2, marginalizeLeaves3) +TEST(ISAM2, marginalizeLeaves3) { ISAM2 isam; @@ -990,7 +990,7 @@ TEST_UNSAFE(ISAM2, marginalizeLeaves3) } /* ************************************************************************* */ -TEST_UNSAFE(ISAM2, marginalizeLeaves4) +TEST(ISAM2, marginalizeLeaves4) { ISAM2 isam; @@ -1017,7 +1017,7 @@ TEST_UNSAFE(ISAM2, marginalizeLeaves4) } /* ************************************************************************* */ -TEST_UNSAFE(ISAM2, marginalizeLeaves5) +TEST(ISAM2, marginalizeLeaves5) { // Create isam2 ISAM2 isam = createSlamlikeISAM2(); diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 2a3eb80fc..a64971b36 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -273,10 +273,10 @@ TEST(NonlinearFactor, NoiseModelFactor4) { EXPECT(assert_equal(Vector_(1, 10.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); - LONGS_EQUAL((long)jf.keys()[0], 0); - LONGS_EQUAL((long)jf.keys()[1], 1); - LONGS_EQUAL((long)jf.keys()[2], 2); - LONGS_EQUAL((long)jf.keys()[3], 3); + LONGS_EQUAL(X(1), (long)jf.keys()[0]); + LONGS_EQUAL(X(2), (long)jf.keys()[1]); + LONGS_EQUAL(X(3), (long)jf.keys()[2]); + LONGS_EQUAL(X(4), (long)jf.keys()[3]); EXPECT(assert_equal(Matrix_(1,1, 0.5), jf.getA(jf.begin()))); EXPECT(assert_equal(Matrix_(1,1, 1.0), jf.getA(jf.begin()+1))); EXPECT(assert_equal(Matrix_(1,1, 1.5), jf.getA(jf.begin()+2))); @@ -320,11 +320,11 @@ TEST(NonlinearFactor, NoiseModelFactor5) { EXPECT(assert_equal(Vector_(1, 15.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); - LONGS_EQUAL((long)jf.keys()[0], 0); - LONGS_EQUAL((long)jf.keys()[1], 1); - LONGS_EQUAL((long)jf.keys()[2], 2); - LONGS_EQUAL((long)jf.keys()[3], 3); - LONGS_EQUAL((long)jf.keys()[4], 4); + LONGS_EQUAL(X(1), (long)jf.keys()[0]); + LONGS_EQUAL(X(2), (long)jf.keys()[1]); + LONGS_EQUAL(X(3), (long)jf.keys()[2]); + LONGS_EQUAL(X(4), (long)jf.keys()[3]); + LONGS_EQUAL(X(5), (long)jf.keys()[4]); EXPECT(assert_equal(Matrix_(1,1, 0.5), jf.getA(jf.begin()))); EXPECT(assert_equal(Matrix_(1,1, 1.0), jf.getA(jf.begin()+1))); EXPECT(assert_equal(Matrix_(1,1, 1.5), jf.getA(jf.begin()+2))); @@ -373,12 +373,12 @@ TEST(NonlinearFactor, NoiseModelFactor6) { EXPECT(assert_equal(Vector_(1, 21.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); - LONGS_EQUAL((long)jf.keys()[0], 0); - LONGS_EQUAL((long)jf.keys()[1], 1); - LONGS_EQUAL((long)jf.keys()[2], 2); - LONGS_EQUAL((long)jf.keys()[3], 3); - LONGS_EQUAL((long)jf.keys()[4], 4); - LONGS_EQUAL((long)jf.keys()[5], 5); + LONGS_EQUAL(X(1), (long)jf.keys()[0]); + LONGS_EQUAL(X(2), (long)jf.keys()[1]); + LONGS_EQUAL(X(3), (long)jf.keys()[2]); + LONGS_EQUAL(X(4), (long)jf.keys()[3]); + LONGS_EQUAL(X(5), (long)jf.keys()[4]); + LONGS_EQUAL(X(6), (long)jf.keys()[5]); EXPECT(assert_equal(Matrix_(1,1, 0.5), jf.getA(jf.begin()))); EXPECT(assert_equal(Matrix_(1,1, 1.0), jf.getA(jf.begin()+1))); EXPECT(assert_equal(Matrix_(1,1, 1.5), jf.getA(jf.begin()+2))); diff --git a/tests/testOccupancyGrid.cpp b/tests/testOccupancyGrid.cpp index 048478ce4..d92e5442e 100644 --- a/tests/testOccupancyGrid.cpp +++ b/tests/testOccupancyGrid.cpp @@ -284,7 +284,7 @@ public: }; /* ************************************************************************* */ -TEST_UNSAFE( OccupancyGrid, Test1) { +TEST( OccupancyGrid, Test1) { //Build a small grid and test optimization //Build small grid diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 461f00405..8bf2c1412 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -58,7 +58,7 @@ TEST( wrap, ArgumentList ) { } /* ************************************************************************* */ -TEST_UNSAFE( wrap, check_exception ) { +TEST( wrap, check_exception ) { THROWS_EXCEPTION(Module("/notarealpath", "geometry",enable_verbose)); CHECK_EXCEPTION(Module("/alsonotarealpath", "geometry",enable_verbose), CantOpenFile);