diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index 448600266..75aa50a25 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -11,11 +11,12 @@ /** * @file testCal3Bundler.cpp - * @brief Unit tests for transform derivatives + * @brief Unit tests for Bundler calibration model. */ #include #include +#include #include #include @@ -28,7 +29,7 @@ static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000); static Point2 p(2,3); /* ************************************************************************* */ -TEST( Cal3Bundler, vector) +TEST(Cal3Bundler, vector) { Cal3Bundler K; Vector expected(3); @@ -37,7 +38,7 @@ TEST( Cal3Bundler, vector) } /* ************************************************************************* */ -TEST( Cal3Bundler, uncalibrate) +TEST(Cal3Bundler, uncalibrate) { Vector v = K.vector() ; double r = p.x()*p.x() + p.y()*p.y() ; @@ -47,7 +48,7 @@ TEST( Cal3Bundler, uncalibrate) CHECK(assert_equal(expected,actual)); } -TEST( Cal3Bundler, calibrate ) +TEST(Cal3Bundler, calibrate ) { Point2 pn(0.5, 0.5); Point2 pi = K.uncalibrate(pn); @@ -61,7 +62,7 @@ Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibra Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { return k.calibrate(pt); } /* ************************************************************************* */ -TEST( Cal3Bundler, Duncalibrate) +TEST(Cal3Bundler, Duncalibrate) { Matrix Dcal, Dp; Point2 actual = K.uncalibrate(p, Dcal, Dp); @@ -74,7 +75,7 @@ TEST( Cal3Bundler, Duncalibrate) } /* ************************************************************************* */ -TEST( Cal3Bundler, Dcalibrate) +TEST(Cal3Bundler, Dcalibrate) { Matrix Dcal, Dp; Point2 pn(0.5, 0.5); @@ -88,22 +89,32 @@ TEST( Cal3Bundler, Dcalibrate) } /* ************************************************************************* */ -TEST( Cal3Bundler, assert_equal) +TEST(Cal3Bundler, assert_equal) { CHECK(assert_equal(K,K,1e-7)); } /* ************************************************************************* */ -TEST( Cal3Bundler, retract) +TEST(Cal3Bundler, retract) { Cal3Bundler expected(510, 2e-3, 2e-3, 1000, 2000); - Vector d(3); + Vector3 d; d << 10, 1e-3, 1e-3; Cal3Bundler actual = K.retract(d); CHECK(assert_equal(expected,actual,1e-7)); CHECK(assert_equal(d,K.localCoordinates(actual),1e-7)); } +/* ************************************************************************* */ +TEST(Cal3_S2, Print) { + Cal3Bundler cal(1, 2, 3, 4, 5); + std::stringstream os; + os << "f: " << cal.fx() << ", k1: " << cal.k1() << ", k2: " << cal.k2() + << ", px: " << cal.px() << ", py: " << cal.py(); + + EXPECT(assert_stdout_equal(os.str(), cal)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCal3DFisheye.cpp b/gtsam/geometry/tests/testCal3DFisheye.cpp index 6bfbe3e46..85e661728 100644 --- a/gtsam/geometry/tests/testCal3DFisheye.cpp +++ b/gtsam/geometry/tests/testCal3DFisheye.cpp @@ -10,12 +10,13 @@ * -------------------------------------------------------------------------- */ /** - * @file testCal3DFisheye.cpp + * @file testCal3Fisheye.cpp * @brief Unit tests for fisheye calibration class * @author ghaggin */ #include +#include #include #include #include @@ -198,6 +199,17 @@ TEST(Cal3Fisheye, Dcalibrate) CHECK(assert_equal(numerical2,Dp,1e-5)); } +/* ************************************************************************* */ +TEST(Cal3Fisheye, Print) { + Cal3Fisheye cal(1, 2, 3, 4, 5, 6, 7, 8, 9); + std::stringstream os; + os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew() + << ", px: " << cal.px() << ", py: " << cal.py() << ", k1: " << cal.k1() + << ", k2: " << cal.k2() << ", k3: " << cal.k3() << ", k4: " << cal.k4(); + + EXPECT(assert_stdout_equal(os.str(), cal)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index beed09883..b382e85f3 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -11,12 +11,13 @@ /** * @file testCal3DS2.cpp - * @brief Unit tests for transform derivatives + * @brief Unit tests for Cal3DS2 calibration model. */ #include #include +#include #include #include @@ -29,7 +30,7 @@ static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3); static Point2 p(2,3); /* ************************************************************************* */ -TEST( Cal3DS2, uncalibrate) +TEST(Cal3DS2, uncalibrate) { Vector k = K.k() ; double r = p.x()*p.x() + p.y()*p.y() ; @@ -43,7 +44,7 @@ TEST( Cal3DS2, uncalibrate) CHECK(assert_equal(q,p_i)); } -TEST( Cal3DS2, calibrate ) +TEST(Cal3DS2, calibrate ) { Point2 pn(0.5, 0.5); Point2 pi = K.uncalibrate(pn); @@ -54,7 +55,7 @@ TEST( Cal3DS2, calibrate ) Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) { return k.uncalibrate(pt); } /* ************************************************************************* */ -TEST( Cal3DS2, Duncalibrate1) +TEST(Cal3DS2, Duncalibrate1) { Matrix computed; K.uncalibrate(p, computed, boost::none); @@ -65,7 +66,7 @@ TEST( Cal3DS2, Duncalibrate1) } /* ************************************************************************* */ -TEST( Cal3DS2, Duncalibrate2) +TEST(Cal3DS2, Duncalibrate2) { Matrix computed; K.uncalibrate(p, boost::none, computed); Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); @@ -79,7 +80,7 @@ Point2 calibrate_(const Cal3DS2& k, const Point2& pt) { } /* ************************************************************************* */ -TEST( Cal3DS2, Dcalibrate) +TEST(Cal3DS2, Dcalibrate) { Point2 pn(0.5, 0.5); Point2 pi = K.uncalibrate(pn); @@ -95,7 +96,7 @@ 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); @@ -106,6 +107,17 @@ TEST( Cal3DS2, retract) CHECK(assert_equal(d,K.localCoordinates(actual),1e-7)); } +/* ************************************************************************* */ +TEST(Cal3DS2, Print) { + Cal3DS2 cal(1, 2, 3, 4, 5, 6, 7, 8, 9); + std::stringstream os; + os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew() + << ", px: " << cal.px() << ", py: " << cal.py() << ", k1: " << cal.k1() + << ", k2: " << cal.k2() << ", p1: " << cal.p1() << ", p2: " << cal.p2(); + + EXPECT(assert_stdout_equal(os.str(), cal)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCal3Unified.cpp b/gtsam/geometry/tests/testCal3Unified.cpp index 8abb6fe04..41a1d3ad9 100644 --- a/gtsam/geometry/tests/testCal3Unified.cpp +++ b/gtsam/geometry/tests/testCal3Unified.cpp @@ -10,12 +10,13 @@ * -------------------------------------------------------------------------- */ /** - * @file testCal3Unify.cpp - * @brief Unit tests for transform derivatives + * @file testCal3Unified.cpp + * @brief Unit tests for Cal3Unified calibration model. */ #include #include +#include #include #include @@ -39,7 +40,7 @@ static Cal3Unified K(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3 static Point2 p(0.5, 0.7); /* ************************************************************************* */ -TEST( Cal3Unified, uncalibrate) +TEST(Cal3Unified, uncalibrate) { Point2 p_i(364.7791831734982, 305.6677211952602) ; Point2 q = K.uncalibrate(p); @@ -47,7 +48,7 @@ TEST( Cal3Unified, uncalibrate) } /* ************************************************************************* */ -TEST( Cal3Unified, spaceNplane) +TEST(Cal3Unified, spaceNplane) { Point2 q = K.spaceToNPlane(p); CHECK(assert_equal(Point2(0.441731600049497, 0.618424240069295), q)); @@ -55,7 +56,7 @@ TEST( Cal3Unified, spaceNplane) } /* ************************************************************************* */ -TEST( Cal3Unified, calibrate) +TEST(Cal3Unified, calibrate) { Point2 pi = K.uncalibrate(p); Point2 pn_hat = K.calibrate(pi); @@ -65,7 +66,7 @@ TEST( Cal3Unified, calibrate) Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { return k.uncalibrate(pt); } /* ************************************************************************* */ -TEST( Cal3Unified, Duncalibrate1) +TEST(Cal3Unified, Duncalibrate1) { Matrix computed; K.uncalibrate(p, computed, boost::none); @@ -74,7 +75,7 @@ TEST( Cal3Unified, Duncalibrate1) } /* ************************************************************************* */ -TEST( Cal3Unified, Duncalibrate2) +TEST(Cal3Unified, Duncalibrate2) { Matrix computed; K.uncalibrate(p, boost::none, computed); @@ -87,7 +88,7 @@ Point2 calibrate_(const Cal3Unified& k, const Point2& pt) { } /* ************************************************************************* */ -TEST( Cal3Unified, Dcalibrate) +TEST(Cal3Unified, Dcalibrate) { Point2 pi = K.uncalibrate(p); Matrix Dcal, Dp; @@ -99,13 +100,13 @@ TEST( Cal3Unified, Dcalibrate) } /* ************************************************************************* */ -TEST( Cal3Unified, assert_equal) +TEST(Cal3Unified, assert_equal) { CHECK(assert_equal(K,K,1e-9)); } /* ************************************************************************* */ -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); @@ -117,7 +118,7 @@ TEST( Cal3Unified, retract) } /* ************************************************************************* */ -TEST( Cal3Unified, DerivedValue) +TEST(Cal3Unified, DerivedValue) { Values values; Cal3Unified cal(1, 2, 3, 4, 5, 6, 7, 8, 9, 10); @@ -129,6 +130,18 @@ TEST( Cal3Unified, DerivedValue) CHECK(assert_equal(cal,calafter,1e-9)); } +/* ************************************************************************* */ +TEST(Cal3Unified, Print) { + Cal3Unified cal(0, 1, 2, 3, 4, 5, 6, 7, 8, 9); + std::stringstream os; + os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew() + << ", px: " << cal.px() << ", py: " << cal.py() << ", k1: " << cal.k1() + << ", k2: " << cal.k2() << ", p1: " << cal.p1() << ", p2: " << cal.p2() + << ", xi: " << cal.xi(); + + EXPECT(assert_stdout_equal(os.str(), cal)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index 64e807aaf..3317addff 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -11,7 +11,7 @@ /** * @file testCal3_S2.cpp - * @brief Unit tests for transform derivatives + * @brief Unit tests for basic Cal3_S2 calibration model. */ #include @@ -31,7 +31,7 @@ static Point2 p_uv(1320.3, 1740); static Point2 p_xy(2, 3); /* ************************************************************************* */ -TEST( Cal3_S2, easy_constructor) +TEST(Cal3_S2, easy_constructor) { Cal3_S2 expected(554.256, 554.256, 0, 640 / 2, 480 / 2); @@ -43,7 +43,7 @@ TEST( Cal3_S2, easy_constructor) } /* ************************************************************************* */ -TEST( Cal3_S2, calibrate) +TEST(Cal3_S2, calibrate) { Point2 intrinsic(2,3); Point2 expectedimage(1320.3, 1740); @@ -53,7 +53,7 @@ TEST( Cal3_S2, calibrate) } /* ************************************************************************* */ -TEST( Cal3_S2, calibrate_homogeneous) { +TEST(Cal3_S2, calibrate_homogeneous) { Vector3 intrinsic(2, 3, 1); Vector3 image(1320.3, 1740, 1); CHECK(assert_equal((Vector)intrinsic,(Vector)K.calibrate(image))); @@ -61,7 +61,7 @@ TEST( Cal3_S2, calibrate_homogeneous) { /* ************************************************************************* */ Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) { return k.uncalibrate(pt); } -TEST( Cal3_S2, Duncalibrate1) +TEST(Cal3_S2, Duncalibrate1) { Matrix25 computed; K.uncalibrate(p, computed, boost::none); Matrix numerical = numericalDerivative21(uncalibrate_, K, p); @@ -69,7 +69,7 @@ TEST( Cal3_S2, Duncalibrate1) } /* ************************************************************************* */ -TEST( Cal3_S2, Duncalibrate2) +TEST(Cal3_S2, Duncalibrate2) { Matrix computed; K.uncalibrate(p, boost::none, computed); Matrix numerical = numericalDerivative22(uncalibrate_, K, p); @@ -98,7 +98,7 @@ TEST(Cal3_S2, Dcalibrate2) } /* ************************************************************************* */ -TEST( Cal3_S2, assert_equal) +TEST(Cal3_S2, assert_equal) { CHECK(assert_equal(K,K,1e-9)); @@ -107,7 +107,7 @@ 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); @@ -131,8 +131,8 @@ TEST(Cal3_S2, between) { TEST(Cal3_S2, Print) { Cal3_S2 cal(5, 5, 5, 5, 5); std::stringstream os; - os << "{ fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew() - << ", px: " << cal.px() << ", py: " << cal.py() << " }"; + os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew() + << ", px: " << cal.px() << ", py: " << cal.py(); EXPECT(assert_stdout_equal(os.str(), cal)); } diff --git a/gtsam/geometry/tests/testCal3_S2Stereo.cpp b/gtsam/geometry/tests/testCal3_S2Stereo.cpp new file mode 100644 index 000000000..9c93b7496 --- /dev/null +++ b/gtsam/geometry/tests/testCal3_S2Stereo.cpp @@ -0,0 +1,136 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testCal3_S2Stereo.cpp + * @brief Unit tests for stereo-rig calibration model. + */ + +#include +#include +#include +#include +#include + +using namespace gtsam; + +GTSAM_CONCEPT_TESTABLE_INST(Cal3_S2Stereo) +GTSAM_CONCEPT_MANIFOLD_INST(Cal3_S2Stereo) + +static Cal3_S2Stereo K(500, 500, 0.1, 640 / 2, 480 / 2, 1); +static Point2 p(1, -2); +static Point2 p_uv(1320.3, 1740); +static Point2 p_xy(2, 3); + +/* ************************************************************************* */ +TEST(Cal3_S2Stereo, easy_constructor) { + Cal3_S2Stereo expected(554.256, 554.256, 0, 640 / 2, 480 / 2, 3); + + double fov = 60; // degrees + size_t w = 640, h = 480; + Cal3_S2Stereo actual(fov, w, h, 3); + + CHECK(assert_equal(expected, actual, 1e-3)); +} + +/* ************************************************************************* */ +TEST(Cal3_S2Stereo, calibrate) { + Point2 intrinsic(2, 3); + Point2 expectedimage(1320.3, 1740); + Point2 imagecoordinates = K.uncalibrate(intrinsic); + CHECK(assert_equal(expectedimage, imagecoordinates)); + CHECK(assert_equal(intrinsic, K.calibrate(imagecoordinates))); +} + +/* ************************************************************************* */ +TEST(Cal3_S2Stereo, calibrate_homogeneous) { + Vector3 intrinsic(2, 3, 1); + Vector3 image(1320.3, 1740, 1); + CHECK(assert_equal((Vector)intrinsic, (Vector)K.calibrate(image))); +} + +//TODO(Varun) Add calibrate and uncalibrate methods +// /* ************************************************************************* */ +// Point2 uncalibrate_(const Cal3_S2Stereo& k, const Point2& pt) { +// return k.uncalibrate(pt); +// } + +// TEST(Cal3_S2Stereo, Duncalibrate1) { +// Matrix26 computed; +// K.uncalibrate(p, computed, boost::none); +// Matrix numerical = numericalDerivative21(uncalibrate_, K, p); +// CHECK(assert_equal(numerical, computed, 1e-8)); +// } + +// /* ************************************************************************* */ +// TEST(Cal3_S2Stereo, 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_S2Stereo& k, const Point2& pt) { +// return k.calibrate(pt); +// } +// /* ************************************************************************* */ +// TEST(Cal3_S2Stereo, Dcalibrate1) { +// Matrix computed; +// Point2 expected = K.calibrate(p_uv, computed, boost::none); +// Matrix numerical = numericalDerivative21(calibrate_, K, p_uv); +// CHECK(assert_equal(expected, p_xy, 1e-8)); +// CHECK(assert_equal(numerical, computed, 1e-8)); +// } + +// /* ************************************************************************* */ +// TEST(Cal3_S2Stereo, Dcalibrate2) { +// Matrix computed; +// Point2 expected = K.calibrate(p_uv, boost::none, computed); +// Matrix numerical = numericalDerivative22(calibrate_, K, p_uv); +// CHECK(assert_equal(expected, p_xy, 1e-8)); +// CHECK(assert_equal(numerical, computed, 1e-8)); +// } + +/* ************************************************************************* */ +TEST(Cal3_S2Stereo, assert_equal) { + CHECK(assert_equal(K, K, 1e-9)); + + Cal3_S2Stereo K1(500, 500, 0.1, 640 / 2, 480 / 2, 1); + CHECK(assert_equal(K, K1, 1e-9)); +} + +/* ************************************************************************* */ +TEST(Cal3_S2Stereo, retract) { + Cal3_S2Stereo expected(500 + 1, 500 + 2, 0.1 + 3, 640 / 2 + 4, 480 / 2 + 5, + 7); + Vector6 d; + d << 1, 2, 3, 4, 5, 6; + Cal3_S2Stereo actual = K.retract(d); + CHECK(assert_equal(expected, actual, 1e-7)); + CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7)); +} + +/* ************************************************************************* */ +TEST(Cal3_S2Stereo, Print) { + Cal3_S2Stereo cal(5, 5, 5, 5, 5, 2); + std::stringstream os; + os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew() + << ", px: " << cal.px() << ", py: " << cal.py() + << ", b: " << cal.baseline(); + EXPECT(assert_stdout_equal(os.str(), cal)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */