diff --git a/gtsam_unstable/nonlinear/tests/testBADFactor.cpp b/gtsam_unstable/nonlinear/tests/testBADFactor.cpp index 3a4c021ed..363d65196 100644 --- a/gtsam_unstable/nonlinear/tests/testBADFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testBADFactor.cpp @@ -30,24 +30,7 @@ using namespace gtsam; /* ************************************************************************* */ -Point3 transformTo(const Pose3& x, const Point3& p, - boost::optional Dpose, boost::optional Dpoint) { - return x.transform_to(p, Dpose, Dpoint); -} - -Point2 project(const Point3& p, boost::optional Dpoint) { - return PinholeCamera::project_to_camera(p, Dpoint); -} - -template -Point2 uncalibrate(const CAL& K, const Point2& p, boost::optional Dcal, - boost::optional Dp) { - return K.uncalibrate(p, Dcal, Dp); -} - -/* ************************************************************************* */ - -TEST(BAD, test) { +TEST(BADFactor, test) { // Create some values Values values; @@ -71,27 +54,21 @@ TEST(BAD, test) { Expression K(3); // Create expression tree - Expression p_cam(transformTo, x, p); - Expression projection(project, p_cam); - Expression uv_hat(uncalibrate, K, projection); + Expression p_cam(x, &Pose3::transform_to, p); + Expression projection(PinholeCamera::project_to_camera, p_cam); + Expression uv_hat(K, &Cal3_S2::uncalibrate, projection); - // Create factor + // Create factor and check value, dimension, linearization BADFactor f(measured, uv_hat); - - // Check value EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9); - - // Check dimension EXPECT_LONGS_EQUAL(0, f.dim()); - - // Check linearization boost::shared_ptr gf = f.linearize(values); EXPECT( assert_equal(*expected, *gf, 1e-9)); } /* ************************************************************************* */ -TEST(BAD, compose) { +TEST(BADFactor, compose) { // Create expression Expression R1(1), R2(2); @@ -115,7 +92,7 @@ TEST(BAD, compose) { /* ************************************************************************* */ // Test compose with arguments referring to the same rotation -TEST(BAD, compose2) { +TEST(BADFactor, compose2) { // Create expression Expression R1(1), R2(1); @@ -129,7 +106,7 @@ TEST(BAD, compose2) { values.insert(1, Rot3()); // Check linearization - JacobianFactor expected(1, 2*eye(3), zero(3)); + JacobianFactor expected(1, 2 * eye(3), zero(3)); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf);