diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index b821d295b..59993819e 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -87,6 +87,20 @@ TEST(Cal3Bundler, Dcalibrate) { CHECK(assert_equal(numerical2, Dp, 1e-5)); } +/* ************************************************************************* */ +TEST(Cal3Bundler, DcalibrateDefault) { + Cal3Bundler trueK(1, 0, 0); + Matrix Dcal, Dp; + Point2 pn(0.5, 0.5); + Point2 pi = trueK.uncalibrate(pn); + Point2 actual = trueK.calibrate(pi, Dcal, Dp); + CHECK(assert_equal(pn, actual, 1e-7)); + Matrix numerical1 = numericalDerivative21(calibrate_, trueK, pi); + Matrix numerical2 = numericalDerivative22(calibrate_, trueK, pi); + CHECK(assert_equal(numerical1, Dcal, 1e-5)); + CHECK(assert_equal(numerical2, Dp, 1e-5)); +} + /* ************************************************************************* */ TEST(Cal3Bundler, assert_equal) { CHECK(assert_equal(K, K, 1e-7)); }