From a423849afe8ed9af6a93d0183fcad05642f55fae Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 12 Oct 2013 14:31:18 +0000 Subject: [PATCH] Comments and formatting, plus some added tests on Cal3Bundler --- gtsam/geometry/Cal3Bundler.cpp | 187 ++++++++++++----------- gtsam/geometry/Cal3Bundler.h | 62 ++++---- gtsam/geometry/Cal3DS2.h | 12 +- gtsam/geometry/tests/testCal3Bundler.cpp | 26 ++-- 4 files changed, 155 insertions(+), 132 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 26e7e3260..212758f71 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -24,131 +24,144 @@ namespace gtsam { /* ************************************************************************* */ -Cal3Bundler::Cal3Bundler() : f_(1), k1_(0), k2_(0) {} - -/* ************************************************************************* */ -Cal3Bundler::Cal3Bundler(const Vector &v) : f_(v(0)), k1_(v(1)), k2_(v(2)) {} - -/* ************************************************************************* */ -Cal3Bundler::Cal3Bundler(const double f, const double k1, const double k2) : f_(f), k1_(k1), k2_(k2) {} - -/* ************************************************************************* */ -Matrix Cal3Bundler::K() const { return Matrix_(3,3, f_, 0.0, 0.0, 0.0, f_, 0.0, 0.0, 0.0, 1.0); } - -/* ************************************************************************* */ -Vector Cal3Bundler::k() const { return Vector_(4, k1_, k2_, 0, 0) ; } - -/* ************************************************************************* */ -Vector Cal3Bundler::vector() const { return Vector_(3, f_, k1_, k2_) ; } - -/* ************************************************************************* */ -void Cal3Bundler::print(const std::string& s) const { gtsam::print(vector(), s + ".K") ; } - -/* ************************************************************************* */ -bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const { - if (fabs(f_ - K.f_) > tol || fabs(k1_ - K.k1_) > tol || fabs(k2_ - K.k2_) > tol) - return false; - return true ; +Cal3Bundler::Cal3Bundler() : + f_(1), k1_(0), k2_(0) { } /* ************************************************************************* */ -Point2 Cal3Bundler::uncalibrate(const Point2& p, - boost::optional H1, - boost::optional H2) const { -// r = x^2 + y^2 ; -// g = (1 + k(1)*r + k(2)*r^2) ; +Cal3Bundler::Cal3Bundler(const Vector &v) : + f_(v(0)), k1_(v(1)), k2_(v(2)) { +} + +/* ************************************************************************* */ +Cal3Bundler::Cal3Bundler(const double f, const double k1, const double k2) : + f_(f), k1_(k1), k2_(k2) { +} + +/* ************************************************************************* */ +Matrix Cal3Bundler::K() const { + return Matrix_(3, 3, f_, 0.0, 0.0, 0.0, f_, 0.0, 0.0, 0.0, 1.0); +} + +/* ************************************************************************* */ +Vector Cal3Bundler::k() const { + return Vector_(4, k1_, k2_, 0, 0); +} + +/* ************************************************************************* */ +Vector Cal3Bundler::vector() const { + return Vector_(3, f_, k1_, k2_); +} + +/* ************************************************************************* */ +void Cal3Bundler::print(const std::string& s) const { + gtsam::print(vector(), s + ".K"); +} + +/* ************************************************************************* */ +bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const { + if (fabs(f_ - K.f_) > tol || fabs(k1_ - K.k1_) > tol + || fabs(k2_ - K.k2_) > tol) + return false; + return true; +} + +/* ************************************************************************* */ +Point2 Cal3Bundler::uncalibrate(const Point2& p, // + boost::optional Dcal, boost::optional Dp) const { +// r = x^2 + y^2; +// g = (1 + k(1)*r + k(2)*r^2); // pi(:,i) = g * pn(:,i) - const double x = p.x(), y = p.y() ; - const double r = x*x + y*y ; - const double r2 = r*r ; - const double g = 1 + k1_ * r + k2_*r2 ; // save one multiply - const double fg = f_*g ; + const double x = p.x(), y = p.y(); + const double r = x * x + y * y; + const double r2 = r * r; + const double g = 1 + (k1_ + k2_ * r) * r; + const double fg = f_ * g; // semantic meaningful version - //if (H1) *H1 = D2d_calibration(p) ; - //if (H2) *H2 = D2d_intrinsic(p) ; + //if (H1) *H1 = D2d_calibration(p); + //if (H2) *H2 = D2d_intrinsic(p); // unrolled version, much faster - if ( H1 || H2 ) { + if (Dcal || Dp) { - const double fx = f_*x, fy = f_*y ; - if ( H1 ) { - *H1 = Matrix_(2, 3, - g*x, fx*r , fx*r2, - g*y, fy*r , fy*r2) ; + const double fx = f_ * x, fy = f_ * y; + if (Dcal) { + *Dcal = Matrix_(2, 3, // + g * x, fx * r, fx * r2, // + g * y, fy * r, fy * r2); } - if ( H2 ) { - const double dr_dx = 2*x ; - const double dr_dy = 2*y ; - const double dg_dx = k1_*dr_dx + k2_*2*r*dr_dx ; - const double dg_dy = k1_*dr_dy + k2_*2*r*dr_dy ; - *H2 = Matrix_(2, 2, - fg + fx*dg_dx, fx*dg_dy, - fy*dg_dx , fg + fy*dg_dy) ; + if (Dp) { + const double dr_dx = 2 * x; + const double dr_dy = 2 * y; + const double dg_dx = k1_ * dr_dx + k2_ * 2 * r * dr_dx; + const double dg_dy = k1_ * dr_dy + k2_ * 2 * r * dr_dy; + *Dp = Matrix_(2, 2, // + fg + fx * dg_dx, fx * dg_dy, // + fy * dg_dx, fg + fy * dg_dy); } } - return Point2(fg * x, fg * y) ; + return Point2(fg * x, fg * y); } /* ************************************************************************* */ Matrix Cal3Bundler::D2d_intrinsic(const Point2& p) const { - const double x = p.x(), y = p.y(), xx = x*x, yy = y*y; - const double r = xx + yy ; - const double dr_dx = 2*x ; - const double dr_dy = 2*y ; - const double g = 1 + (k1_+k2_*r) * r ; // save one multiply - //const double g = 1 + k1_*r + k2_*r*r ; - const double dg_dx = k1_*dr_dx + k2_*2*r*dr_dx ; - const double dg_dy = k1_*dr_dy + k2_*2*r*dr_dy ; + const double x = p.x(), y = p.y(), xx = x * x, yy = y * y; + const double r = xx + yy; + const double dr_dx = 2 * x; + const double dr_dy = 2 * y; + const double g = 1 + (k1_ + k2_ * r) * r; + const double dg_dx = k1_ * dr_dx + k2_ * 2 * r * dr_dx; + const double dg_dy = k1_ * dr_dy + k2_ * 2 * r * dr_dy; Matrix DK = Matrix_(2, 2, f_, 0.0, 0.0, f_); - Matrix DR = Matrix_(2, 2, - g + x*dg_dx, x*dg_dy, - y*dg_dx , g + y*dg_dy) ; + Matrix DR = Matrix_(2, 2, g + x * dg_dx, x * dg_dy, y * dg_dx, g + y * dg_dy); return DK * DR; } /* ************************************************************************* */ Matrix Cal3Bundler::D2d_calibration(const Point2& p) const { - const double x = p.x(), y = p.y(), xx = x*x, yy = y*y ; - const double r = xx + yy ; - const double r2 = r*r ; - const double f = f_ ; - const double g = 1 + (k1_+k2_*r) * r ; // save one multiply - //const double g = (1+k1_*r+k2_*r2) ; + const double x = p.x(), y = p.y(), xx = x * x, yy = y * y; + const double r = xx + yy; + const double r2 = r * r; + const double f = f_; + const double g = 1 + (k1_ + k2_ * r) * r; - return Matrix_(2, 3, - g*x, f*x*r , f*x*r2, - g*y, f*y*r , f*y*r2); + return Matrix_(2, 3, g * x, f * x * r, f * x * r2, g * y, f * y * r, + f * y * r2); } /* ************************************************************************* */ Matrix Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const { - const double x = p.x(), y = p.y(), xx = x*x, yy = y*y; - const double r = xx + yy ; - const double r2 = r*r; - const double dr_dx = 2*x ; - const double dr_dy = 2*y ; - const double g = 1 + (k1_*r) + (k2_*r2) ; - const double f = f_ ; - const double dg_dx = k1_*dr_dx + k2_*2*r*dr_dx ; - const double dg_dy = k1_*dr_dy + k2_*2*r*dr_dy ; + const double x = p.x(), y = p.y(), xx = x * x, yy = y * y; + const double r = xx + yy; + const double r2 = r * r; + const double dr_dx = 2 * x; + const double dr_dy = 2 * y; + const double g = 1 + (k1_ + k2_ * r) * r; + const double f = f_; + const double dg_dx = k1_ * dr_dx + k2_ * 2 * r * dr_dx; + const double dg_dy = k1_ * dr_dy + k2_ * 2 * r * dr_dy; - Matrix H = Matrix_(2,5, - f*(g + x*dg_dx), f*(x*dg_dy), g*x, f*x*r , f*x*r2, - f*(y*dg_dx) , f*(g + y*dg_dy), g*y, f*y*r , f*y*r2); + Matrix H = Matrix_(2, 5, f * (g + x * dg_dx), f * (x * dg_dy), g * x, + f * x * r, f * x * r2, f * (y * dg_dx), f * (g + y * dg_dy), g * y, + f * y * r, f * y * r2); - return H ; + return H; } /* ************************************************************************* */ -Cal3Bundler Cal3Bundler::retract(const Vector& d) const { return Cal3Bundler(vector() + d) ; } +Cal3Bundler Cal3Bundler::retract(const Vector& d) const { + return Cal3Bundler(vector() + d); +} /* ************************************************************************* */ -Vector Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const { return vector() - T2.vector(); } +Vector Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const { + return vector() - T2.vector(); +} } diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index a39d7a4e5..034afd72a 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -13,7 +13,7 @@ * @file Cal3Bundler.h * @brief Calibration used by Bundler * @date Sep 25, 2010 - * @author ydjian + * @author Yong Dian Jian */ #pragma once @@ -31,30 +31,31 @@ namespace gtsam { class GTSAM_EXPORT Cal3Bundler : public DerivedValue { private: - double f_, k1_, k2_ ; + double f_; ///< focal length + double k1_, k2_;///< radial distortion public: - Matrix K() const ; - Vector k() const ; + Matrix K() const;///< Standard 3*3 calibration matrix + Vector k() const;///< Radial distortion parameters Vector vector() const; /// @name Standard Constructors /// @{ - ///TODO: comment - Cal3Bundler() ; + /// Default constructor + Cal3Bundler(); - ///TODO: comment - Cal3Bundler(const double f, const double k1, const double k2) ; + /// Constructor + Cal3Bundler(const double f, const double k1, const double k2); /// @} /// @name Advanced Constructors /// @{ - ///TODO: comment - Cal3Bundler(const Vector &v) ; + /// Construct from vector + Cal3Bundler(const Vector &v); /// @} /// @name Testable @@ -70,35 +71,41 @@ public: /// @name Standard Interface /// @{ - ///TODO: comment + /** + * convert intrinsic coordinates xy to image coordinates uv + * @param p point in intrinsic coordinates + * @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in image coordinates + */ Point2 uncalibrate(const Point2& p, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const ; + boost::optional Dcal = boost::none, + boost::optional Dp = boost::none) const; - ///TODO: comment - Matrix D2d_intrinsic(const Point2& p) const ; + /// 2*2 Jacobian of uncalibrate with respect to intrinsic coordinates + Matrix D2d_intrinsic(const Point2& p) const; - ///TODO: comment - Matrix D2d_calibration(const Point2& p) const ; + /// 2*3 Jacobian of uncalibrate wrpt CalBundler parameters + Matrix D2d_calibration(const Point2& p) const; - ///TODO: comment - Matrix D2d_intrinsic_calibration(const Point2& p) const ; + /// 2*5 Jacobian of uncalibrate wrpt both intrinsic and calibration + Matrix D2d_intrinsic_calibration(const Point2& p) const; /// @} /// @name Manifold /// @{ - ///TODO: comment - Cal3Bundler retract(const Vector& d) const ; + /// Update calibration with tangent space delta + Cal3Bundler retract(const Vector& d) const; - ///TODO: comment - Vector localCoordinates(const Cal3Bundler& T2) const ; + /// Calculate local coordinates to another calibration + Vector localCoordinates(const Cal3Bundler& T2) const; - ///TODO: comment - virtual size_t dim() const { return 3 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) + /// dimensionality + virtual size_t dim() const {return 3;} - ///TODO: comment - static size_t Dim() { return 3; } //TODO: make a final dimension variable + /// dimensionality + static size_t Dim() {return 3;} private: @@ -118,7 +125,6 @@ private: ar & BOOST_SERIALIZATION_NVP(k2_); } - /// @} }; diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 32a4654cb..5453e1481 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -92,10 +92,16 @@ public: /// image center in y inline double py() const { return v0_;} - /// Convert ideal point p (in intrinsic coordinates) into pixel coordinates + /** + * convert intrinsic coordinates xy to image coordinates uv + * @param p point in intrinsic coordinates + * @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in image coordinates + */ Point2 uncalibrate(const Point2& p, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const ; + boost::optional Dcal = boost::none, + boost::optional Dp = boost::none) const ; /// Conver a pixel coordinate to ideal coordinate Point2 calibrate(const Point2& p, const double tol=1e-5) const; diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index e354796fa..67471e880 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -39,24 +39,22 @@ TEST( Cal3Bundler, calibrate) CHECK(assert_equal(q,q_hat)); } - Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); } /* ************************************************************************* */ -TEST( Cal3Bundler, Duncalibrate1) +TEST( Cal3Bundler, Duncalibrate) { - 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)); + Matrix Dcal, Dp; + K.uncalibrate(p, Dcal, Dp); + Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p); + Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p); + CHECK(assert_equal(numerical1,Dcal,1e-7)); + CHECK(assert_equal(numerical2,Dp,1e-7)); + CHECK(assert_equal(numerical1,K.D2d_calibration(p),1e-7)); + CHECK(assert_equal(numerical2,K.D2d_intrinsic(p),1e-7)); + Matrix Dcombined(2,5); + Dcombined << Dp, Dcal; + CHECK(assert_equal(Dcombined,K.D2d_intrinsic_calibration(p),1e-7)); } /* ************************************************************************* */