diff --git a/gtsam_unstable/base/tests/testBAD.cpp b/gtsam_unstable/base/tests/testBAD.cpp index d8d95ed92..4320ce931 100644 --- a/gtsam_unstable/base/tests/testBAD.cpp +++ b/gtsam_unstable/base/tests/testBAD.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -101,11 +102,23 @@ public: TEST(BAD, test) { + // Create some values + Values values; + values.insert(1,Pose3()); + values.insert(2,Point3(0,0,1)); + values.insert(3,Cal3_S2()); + + // Create old-style factor to create expected value and derivatives + Point2 measured(0,1); + SharedNoiseModel model = noiseModel::Unit::Create(2); + GeneralSFMFactor2 old(measured, model, 1, 2, 3); + GaussianFactor::shared_ptr expected = old.linearize(values); + // Create leaves Expression x(1); Expression p(2); Expression K(3); - Expression uv(Point2(300, 62)); + Expression uv(measured); // Create expression tree Expression p_cam = transformTo(x, p); @@ -116,25 +129,15 @@ TEST(BAD, test) { // Create factor BADFactor f(e); - // Create some values - Values values; - // Check value - EXPECT_DOUBLES_EQUAL(0, f.error(values), 1e-9); + EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); // Check dimension EXPECT_LONGS_EQUAL(0, f.dim()); // Check linearization - JacobianFactor expected( // - 1, (Matrix(2, 6) << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12), // - 2, (Matrix(2, 3) << 1, 2, 3, 4, 5, 6), // - 3, (Matrix(2, 5) << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10), // - (Vector(2) << 1, 2)); boost::shared_ptr gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); + EXPECT( assert_equal(*expected, *gf, 1e-9)); } /* ************************************************************************* */