diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index eac8d1044..8e6fe983f 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -93,6 +93,9 @@ TEST(Cal3Bundler, retract) { Cal3Bundler expected(510, 2e-3, 2e-3, 1000, 2000); EXPECT_LONGS_EQUAL(3, expected.dim()); + EXPECT_LONGS_EQUAL(Cal3Bundler::Dim(), 3); + EXPECT_LONGS_EQUAL(expected.dim(), 3); + Vector3 d; d << 10, 1e-3, 1e-3; Cal3Bundler actual = K.retract(d); diff --git a/gtsam/geometry/tests/testCal3DFisheye.cpp b/gtsam/geometry/tests/testCal3DFisheye.cpp index 85e661728..7a73e7490 100644 --- a/gtsam/geometry/tests/testCal3DFisheye.cpp +++ b/gtsam/geometry/tests/testCal3DFisheye.cpp @@ -42,7 +42,11 @@ TEST(Cal3Fisheye, retract) { Cal3Fisheye expected(K.fx() + 1, K.fy() + 2, K.skew() + 3, K.px() + 4, K.py() + 5, K.k1() + 6, K.k2() + 7, K.k3() + 8, K.k4() + 9); - Vector d(9); + + EXPECT_LONGS_EQUAL(Cal3Fisheye::Dim(), 9); + EXPECT_LONGS_EQUAL(expected.dim(), 9); + + Vector9 d; d << 1, 2, 3, 4, 5, 6, 7, 8, 9; Cal3Fisheye actual = K.retract(d); CHECK(assert_equal(expected, actual, 1e-7)); diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index b382e85f3..e4dc3e806 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -96,11 +96,14 @@ TEST(Cal3DS2, Dcalibrate) TEST(Cal3DS2, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); } /* ************************************************************************* */ -TEST(Cal3DS2, retract) -{ +TEST(Cal3DS2, retract) { Cal3DS2 expected(500 + 1, 100 + 2, 0.1 + 3, 320 + 4, 240 + 5, 1e-3 + 6, 2.0 * 1e-3 + 7, 3.0 * 1e-3 + 8, 4.0 * 1e-3 + 9); - Vector d(9); + + EXPECT_LONGS_EQUAL(Cal3DS2::Dim(), 9); + EXPECT_LONGS_EQUAL(expected.dim(), 9); + + Vector9 d; d << 1,2,3,4,5,6,7,8,9; Cal3DS2 actual = K.retract(d); CHECK(assert_equal(expected,actual,1e-7)); diff --git a/gtsam/geometry/tests/testCal3Unified.cpp b/gtsam/geometry/tests/testCal3Unified.cpp index 41a1d3ad9..ff759d1cd 100644 --- a/gtsam/geometry/tests/testCal3Unified.cpp +++ b/gtsam/geometry/tests/testCal3Unified.cpp @@ -106,11 +106,14 @@ TEST(Cal3Unified, assert_equal) } /* ************************************************************************* */ -TEST(Cal3Unified, retract) -{ +TEST(Cal3Unified, retract) { Cal3Unified expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1); - Vector d(10); + + EXPECT_LONGS_EQUAL(Cal3Unified::Dim(), 10); + EXPECT_LONGS_EQUAL(expected.dim(), 10); + + Vector10 d; d << 2, 3, 4, 5, 6, 7, 8, 9, 10, 1; Cal3Unified actual = K.retract(d); CHECK(assert_equal(expected,actual,1e-9)); diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index 3317addff..9b7941c91 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -31,8 +31,7 @@ static Point2 p_uv(1320.3, 1740); static Point2 p_xy(2, 3); /* ************************************************************************* */ -TEST(Cal3_S2, easy_constructor) -{ +TEST(Cal3_S2, Constructor) { Cal3_S2 expected(554.256, 554.256, 0, 640 / 2, 480 / 2); double fov = 60; // degrees @@ -43,8 +42,7 @@ TEST(Cal3_S2, easy_constructor) } /* ************************************************************************* */ -TEST(Cal3_S2, calibrate) -{ +TEST(Cal3_S2, Calibrate) { Point2 intrinsic(2,3); Point2 expectedimage(1320.3, 1740); Point2 imagecoordinates = K.uncalibrate(intrinsic); @@ -53,33 +51,36 @@ TEST(Cal3_S2, calibrate) } /* ************************************************************************* */ -TEST(Cal3_S2, calibrate_homogeneous) { +TEST(Cal3_S2, CalibrateHomogeneous) { Vector3 intrinsic(2, 3, 1); Vector3 image(1320.3, 1740, 1); CHECK(assert_equal((Vector)intrinsic,(Vector)K.calibrate(image))); } /* ************************************************************************* */ -Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) { return k.uncalibrate(pt); } -TEST(Cal3_S2, Duncalibrate1) -{ +Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) { + return k.uncalibrate(pt); +} + +TEST(Cal3_S2, Duncalibrate1) { Matrix25 computed; K.uncalibrate(p, computed, boost::none); Matrix numerical = numericalDerivative21(uncalibrate_, K, p); CHECK(assert_equal(numerical,computed,1e-8)); } /* ************************************************************************* */ -TEST(Cal3_S2, Duncalibrate2) -{ +TEST(Cal3_S2, Duncalibrate2) { Matrix computed; K.uncalibrate(p, boost::none, computed); Matrix numerical = numericalDerivative22(uncalibrate_, K, p); CHECK(assert_equal(numerical,computed,1e-9)); } -Point2 calibrate_(const Cal3_S2& k, const Point2& pt) {return k.calibrate(pt); } +Point2 calibrate_(const Cal3_S2& k, const Point2& pt) { + return k.calibrate(pt); +} + /* ************************************************************************* */ -TEST(Cal3_S2, Dcalibrate1) -{ +TEST(Cal3_S2, Dcalibrate1) { Matrix computed; Point2 expected = K.calibrate(p_uv, computed, boost::none); Matrix numerical = numericalDerivative21(calibrate_, K, p_uv); @@ -88,8 +89,7 @@ TEST(Cal3_S2, Dcalibrate1) } /* ************************************************************************* */ -TEST(Cal3_S2, Dcalibrate2) -{ +TEST(Cal3_S2, Dcalibrate2) { Matrix computed; Point2 expected = K.calibrate(p_uv, boost::none, computed); Matrix numerical = numericalDerivative22(calibrate_, K, p_uv); @@ -98,8 +98,7 @@ TEST(Cal3_S2, Dcalibrate2) } /* ************************************************************************* */ -TEST(Cal3_S2, assert_equal) -{ +TEST(Cal3_S2, Equal) { CHECK(assert_equal(K,K,1e-9)); Cal3_S2 K1(500, 500, 0.1, 640 / 2, 480 / 2); @@ -107,10 +106,13 @@ TEST(Cal3_S2, assert_equal) } /* ************************************************************************* */ -TEST(Cal3_S2, retract) -{ +TEST(Cal3_S2, Retract) { Cal3_S2 expected(500+1, 500+2, 0.1+3, 640 / 2+4, 480 / 2+5); - Vector d(5); + + EXPECT_LONGS_EQUAL(Cal3_S2::Dim(), 5); + EXPECT_LONGS_EQUAL(expected.dim(), 5); + + Vector5 d; d << 1,2,3,4,5; Cal3_S2 actual = K.retract(d); CHECK(assert_equal(expected,actual,1e-7)); diff --git a/gtsam/geometry/tests/testCal3_S2Stereo.cpp b/gtsam/geometry/tests/testCal3_S2Stereo.cpp index 9c93b7496..f823a8b97 100644 --- a/gtsam/geometry/tests/testCal3_S2Stereo.cpp +++ b/gtsam/geometry/tests/testCal3_S2Stereo.cpp @@ -111,6 +111,9 @@ TEST(Cal3_S2Stereo, assert_equal) { TEST(Cal3_S2Stereo, retract) { Cal3_S2Stereo expected(500 + 1, 500 + 2, 0.1 + 3, 640 / 2 + 4, 480 / 2 + 5, 7); + EXPECT_LONGS_EQUAL(Cal3_S2Stereo::Dim(), 6); + EXPECT_LONGS_EQUAL(expected.dim(), 6); + Vector6 d; d << 1, 2, 3, 4, 5, 6; Cal3_S2Stereo actual = K.retract(d);