diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index aa06a2e29..668b69a55 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -26,6 +26,8 @@ GTSAM_CONCEPT_MANIFOLD_INST(Cal3_S2) static Cal3_S2 K(500, 500, 0.1, 640 / 2, 480 / 2); static Point2 p(1, -2); +static Point2 p_uv(1320.3, 1740); +static Point2 p_xy(2, 3); /* ************************************************************************* */ TEST( Cal3_S2, easy_constructor) @@ -73,6 +75,33 @@ TEST( Cal3_S2, Duncalibrate2) CHECK(assert_equal(numerical,computed,1e-9)); } +Point2 calibrate_(const Cal3_S2& k, const Point2& pt) {return k.calibrate(pt); } +/* ************************************************************************* */ +TEST(Cal3_S2, Dcalibrate1) +{ + Matrix computed; + Point2 expected = K.calibrate(p_uv, computed, boost::none); + Matrix numerical = numericalDerivative21(calibrate_, K, p); + CHECK(assert_equal(numerical, computed, 1e-8)); + CHECK(assert_equal(expected, p_xy, 1e-8)); +} + +/* ************************************************************************* */ +TEST(Cal3_S2, Dcalibrate1) +{ + Matrix computed; + Point2 expected = K.calibrate(p_uv, boost::none, computed); + Matrix numerical = numericalDerivative22(calibrate_, K, p); + CHECK(assert_equal(numerical, computed, 1e-8)); + CHECK(assert_equal(expected, p_xy, 1e-8)); +} + +/* ************************************************************************* */ +TEST(Cal3_S2, Dcalibrate2) +{ + +} + /* ************************************************************************* */ TEST( Cal3_S2, assert_equal) {