From 638a6e6917781f6f5d23dc3feb4cb41e22f432b6 Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Tue, 28 Dec 2010 19:08:47 +0000 Subject: [PATCH] bug fix for Cal3DS2, and unit tests for Cal3Bundler + Cal3DS2 --- gtsam/geometry/Cal3DS2.cpp | 8 ++- gtsam/geometry/Cal3DS2.h | 4 +- gtsam/geometry/Makefile.am | 2 +- gtsam/geometry/tests/testCal3Bundler.cpp | 66 +++++++++++++++++++++++ gtsam/geometry/tests/testCal3DS2.cpp | 69 ++++++++++++++++++++++++ 5 files changed, 143 insertions(+), 6 deletions(-) create mode 100644 gtsam/geometry/tests/testCal3Bundler.cpp create mode 100644 gtsam/geometry/tests/testCal3DS2.cpp diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index c2376b72e..8ae85739a 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -102,10 +102,14 @@ Matrix Cal3DS2::D2d_calibration(const Point2& p) const { const double r2 = r*r ; const double fx = fx_, fy = fy_, s = s_ ; const double g = (1+k1_*r+k2_*r2) ; + const double dx = 2*k3_*xy + k4_*(r+2*xx) ; + const double dy = 2*k4_*xy + k3_*(r+2*yy) ; + const double pnx = g*x + dx; + const double pny = g*y + dy; return Matrix_(2, 9, - g*x, 0.0, g*y, 1.0, 0.0, fx*x*r + s*y*r, fx*x*r2 + s*y*r2, fx*2*xy + s*(r+2*yy), fx*(r+2*xx) + s*(2*xy), - 0.0, g*y, 0.0, 0.0, 1.0, fy*y*r , fy*y*r2 , fy*(r+2*yy) , fy*(2*xy) ); + pnx, 0.0, pny, 1.0, 0.0, fx*x*r + s*y*r, fx*x*r2 + s*y*r2, fx*2*xy + s*(r+2*yy), fx*(r+2*xx) + s*(2*xy), + 0.0, pny, 0.0, 0.0, 1.0, fy*y*r , fy*y*r2 , fy*(r+2*yy) , fy*(2*xy) ); } Cal3DS2 Cal3DS2::expmap(const Vector& d) const { return Cal3DS2(vector() + d) ; } diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 95e4f8f45..c3578c21e 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -18,8 +18,6 @@ #pragma once -#ifndef CAL3DS2_H_ -#define CAL3DS2_H_ #include #include @@ -38,6 +36,7 @@ namespace gtsam { // r = Pn.x^2 + Pn.y^2 // \hat{pn} = (1 + k1*r + k2*r^2 ) pn + [ 2*k3 pn.x pn.y + k4 (r + 2 Pn.x^2) ; // k3 (r + 2 Pn.y^2) + 2*k4 pn.x pn.y ] + // pi = K*pn public: // Default Constructor with only unit focal length @@ -92,4 +91,3 @@ namespace gtsam { }; } -#endif /* CAL3DS2_H_ */ diff --git a/gtsam/geometry/Makefile.am b/gtsam/geometry/Makefile.am index ab65acf81..4ed7f7b77 100644 --- a/gtsam/geometry/Makefile.am +++ b/gtsam/geometry/Makefile.am @@ -17,7 +17,7 @@ check_PROGRAMS += tests/testPoint2 tests/testRot2 tests/testPose2 tests/testPoin # Cameras headers += GeneralCameraT.h sources += Cal3_S2.cpp Cal3DS2.cpp Cal3Bundler.cpp CalibratedCamera.cpp SimpleCamera.cpp -check_PROGRAMS += tests/testCal3_S2 tests/testCalibratedCamera tests/testSimpleCamera +check_PROGRAMS += tests/testCal3DS2 tests/testCal3_S2 tests/testCal3Bundler tests/testCalibratedCamera tests/testSimpleCamera # Stereo sources += StereoPoint2.cpp StereoCamera.cpp diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp new file mode 100644 index 000000000..096450cf1 --- /dev/null +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -0,0 +1,66 @@ +/* ---------------------------------------------------------------------------- + + * 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 testCal3Bundler.cpp + * @brief Unit tests for transform derivatives + */ + + +#include +#include +#include + +using namespace gtsam; + +Cal3Bundler K(500, 1e-3, 1e-3); +Point2 p(2,3); + +/* ************************************************************************* */ +TEST( Cal3Bundler, calibrate) +{ + Vector v = K.vector() ; + double r = p.x()*p.x() + p.y()*p.y() ; + double g = v[0]*(1+v[1]*r+v[2]*r*r) ; + Point2 q_hat (g*p.x(), g*p.y()) ; + Point2 q = K.uncalibrate(p); + CHECK(assert_equal(q,q_hat)); +} + + +Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); } + +/* ************************************************************************* */ +TEST( Cal3Bundler, Duncalibrate1) +{ + Matrix computed; + K.uncalibrate(p, computed, boost::none); + Matrix numerical = numericalDerivative21(uncalibrate_, K, p); + CHECK(assert_equal(numerical,computed,1e-7)); +} + +/* ************************************************************************* */ +TEST( Cal3Bundler, Duncalibrate2) +{ + Matrix computed; K.uncalibrate(p, boost::none, computed); + Matrix numerical = numericalDerivative22(uncalibrate_, K, p); + CHECK(assert_equal(numerical,computed,1e-7)); +} + +/* ************************************************************************* */ +TEST( Cal3Bundler, assert_equal) +{ + CHECK(assert_equal(K,K,1e-7)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp new file mode 100644 index 000000000..4cb14c30b --- /dev/null +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -0,0 +1,69 @@ +/* ---------------------------------------------------------------------------- + + * 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 testCal3DS2.cpp + * @brief Unit tests for transform derivatives + */ + + +#include +#include +#include + +using namespace gtsam; + +Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3); +Point2 p(2,3); + +/* ************************************************************************* */ +TEST( Cal3DS2, calibrate) +{ + Vector k = K.k() ; + double r = p.x()*p.x() + p.y()*p.y() ; + double g = 1+k[0]*r+k[1]*r*r ; + double tx = 2*k[2]*p.x()*p.y() + k[3]*(r+2*p.x()*p.x()) ; + double ty = k[2]*(r+2*p.y()*p.y()) + 2*k[3]*p.x()*p.y() ; + Vector v_hat = Vector_(3, g*p.x() + tx, g*p.y() + ty, 1.0) ; + Vector v_i = K.K() * v_hat ; + Point2 p_i(v_i(0)/v_i(2), v_i(1)/v_i(2)) ; + Point2 q = K.uncalibrate(p); + CHECK(assert_equal(q,p_i)); +} + +Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) { return k.uncalibrate(pt); } + +/* ************************************************************************* */ +TEST( Cal3DS2, Duncalibrate1) +{ + Matrix computed; + K.uncalibrate(p, computed, boost::none); + Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); + CHECK(assert_equal(numerical,computed,1e-5)); +} + +/* ************************************************************************* */ +TEST( Cal3DS2, Duncalibrate2) +{ + Matrix computed; K.uncalibrate(p, boost::none, computed); + Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); + CHECK(assert_equal(numerical,computed,1e-5)); +} + +/* ************************************************************************* */ +TEST( Cal3DS2, assert_equal) +{ + CHECK(assert_equal(K,K,1e-5)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */