From fb784eea9c6033ae852ec3af2a9266a568c33830 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 2 Jun 2021 12:12:35 -0400 Subject: [PATCH] add all of Akshay's tests for default model --- gtsam/geometry/tests/testCal3Bundler.cpp | 41 ++++++++++++++++-------- 1 file changed, 27 insertions(+), 14 deletions(-) diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index 59993819e..a0629c83e 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -62,6 +62,33 @@ Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { return k.calibrate(pt); } +/* ************************************************************************* */ +TEST(Cal3Bundler, DuncalibrateDefault) { + Cal3Bundler trueK(1, 0, 0); + Matrix Dcal, Dp; + Point2 actual = trueK.uncalibrate(p, Dcal, Dp); + Point2 expected = p; + CHECK(assert_equal(expected, actual, 1e-7)); + Matrix numerical1 = numericalDerivative21(uncalibrate_, trueK, p); + Matrix numerical2 = numericalDerivative22(uncalibrate_, trueK, p); + CHECK(assert_equal(numerical1, Dcal, 1e-7)); + CHECK(assert_equal(numerical2, Dp, 1e-7)); +} + +/* ************************************************************************* */ +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, Duncalibrate) { Matrix Dcal, Dp; @@ -87,20 +114,6 @@ 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)); }