From 8604bc7328b235a9c50d4af747932e257ab4d31f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 12 Oct 2013 16:26:00 +0000 Subject: [PATCH] Added optional image center to Cal3Bundler, used only as a convenience constant so one does not need to transform the image coordinates. --- gtsam/geometry/Cal3Bundler.cpp | 107 +++++++-------------- gtsam/geometry/Cal3Bundler.h | 60 ++++++------ gtsam/geometry/tests/testCal3Bundler.cpp | 13 +-- gtsam/geometry/tests/timePinholeCamera.cpp | 22 ++--- 4 files changed, 87 insertions(+), 115 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 4af546fac..9f9278831 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -29,18 +29,15 @@ Cal3Bundler::Cal3Bundler() : } /* ************************************************************************* */ -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) { +Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0) : + f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0) { } /* ************************************************************************* */ Matrix Cal3Bundler::K() const { - return Matrix_(3, 3, f_, 0.0, 0.0, 0.0, f_, 0.0, 0.0, 0.0, 1.0); + Matrix3 K; + K << f_, 0, u0_, 0, f_, v0_, 0, 0, 1; + return K; } /* ************************************************************************* */ @@ -55,13 +52,14 @@ Vector Cal3Bundler::vector() const { /* ************************************************************************* */ void Cal3Bundler::print(const std::string& s) const { - gtsam::print(vector(), s + ".K"); + gtsam::print(Vector_(5, f_, k1_, k2_, u0_, v0_), 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) + || fabs(k2_ - K.k2_) > tol || fabs(u0_ - K.u0_) > tol + || fabs(v0_ - K.v0_) > tol) return false; return true; } @@ -69,92 +67,59 @@ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const { /* ************************************************************************* */ 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) + // 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 g = 1. + (k1_ + k2_ * r) * r; const double u = g * x, v = g * y; - const double f = f_; - // semantic meaningful version - //if (H1) *H1 = D2d_calibration(p); - //if (H2) *H2 = D2d_intrinsic(p); - - // unrolled version, much faster - if (Dcal || Dp) { + // Derivatives make use of intermediate variables above + if (Dcal) { double rx = r * x, ry = r * y; - - if (Dcal) { - Eigen::Matrix D; - D << u, f * rx, f * r * rx, v, f * ry, f * r * ry; - *Dcal = D; - } - - if (Dp) { - const double dg_dx = 2. * k1_ * x + 4. * k2_ * rx; - const double dg_dy = 2. * k1_ * y + 4. * k2_ * ry; - Eigen::Matrix D; - D << g + x * dg_dx, x * dg_dy, y * dg_dx, g + y * dg_dy; - *Dp = f * D; - } + Eigen::Matrix D; + D << u, f_ * rx, f_ * r * rx, v, f_ * ry, f_ * r * ry; + *Dcal = D; } - return Point2(f * u, f * v); + if (Dp) { + const double a = 2. * (k1_ + 2. * k2_ * r); + const double axx = a * x * x, axy = a * x * y, ayy = a * y * y; + Eigen::Matrix D; + D << g + axx, axy, axy, g + ayy; + *Dp = f_ * D; + } + + return Point2(u0_ + f_ * u, v0_ + f_ * v); } /* ************************************************************************* */ 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; - 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); - return DK * DR; + Matrix Dp; + uncalibrate(p, boost::none, Dp); + return Dp; } /* ************************************************************************* */ 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; - - return Matrix_(2, 3, g * x, f * x * r, f * x * r2, g * y, f * y * r, - f * y * r2); + Matrix Dcal; + uncalibrate(p, Dcal, boost::none); + return Dcal; } /* ************************************************************************* */ 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_ + 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 Dcal, Dp; + uncalibrate(p, Dcal, Dp); + Matrix H(2, 5); + H << Dp, Dcal; return H; } /* ************************************************************************* */ Cal3Bundler Cal3Bundler::retract(const Vector& d) const { - return Cal3Bundler(vector() + d); + return Cal3Bundler(f_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 034afd72a..641ef2745 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -28,16 +28,17 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Cal3Bundler : public DerivedValue { +class GTSAM_EXPORT Cal3Bundler: public DerivedValue { private: double f_; ///< focal length - double k1_, k2_;///< radial distortion + double k1_, k2_; ///< radial distortion + double u0_, v0_; ///< image center, not a parameter to be optimized but a constant public: - Matrix K() const;///< Standard 3*3 calibration matrix - Vector k() const;///< Radial distortion parameters + Matrix K() const; ///< Standard 3*3 calibration matrix + Vector k() const; ///< Radial distortion parameters (4 of them, 2 0) Vector vector() const; @@ -47,15 +48,15 @@ public: /// Default constructor Cal3Bundler(); - /// Constructor - Cal3Bundler(const double f, const double k1, const double k2); - - /// @} - /// @name Advanced Constructors - /// @{ - - /// Construct from vector - Cal3Bundler(const Vector &v); + /** + * Constructor + * @param f focal length + * @param k1 first radial distortion coefficient (quadratic) + * @param k2 second radial distortion coefficient (quartic) + * @param u0 optional image center (default 0), considered a constant + * @param v0 optional image center (default 0), considered a constant + */ + Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0); /// @} /// @name Testable @@ -78,17 +79,16 @@ public: * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ - Point2 uncalibrate(const Point2& p, - boost::optional Dcal = boost::none, - boost::optional Dp = boost::none) const; + Point2 uncalibrate(const Point2& p, boost::optional Dcal = + boost::none, boost::optional Dp = boost::none) const; - /// 2*2 Jacobian of uncalibrate with respect to intrinsic coordinates + /// @deprecated might be removed in next release, use uncalibrate Matrix D2d_intrinsic(const Point2& p) const; - /// 2*3 Jacobian of uncalibrate wrpt CalBundler parameters + /// @deprecated might be removed in next release, use uncalibrate Matrix D2d_calibration(const Point2& p) const; - /// 2*5 Jacobian of uncalibrate wrpt both intrinsic and calibration + /// @deprecated might be removed in next release, use uncalibrate Matrix D2d_intrinsic_calibration(const Point2& p) const; /// @} @@ -102,10 +102,14 @@ public: Vector localCoordinates(const Cal3Bundler& T2) const; /// dimensionality - virtual size_t dim() const {return 3;} + virtual size_t dim() const { + return 3; + } /// dimensionality - static size_t Dim() {return 3;} + static size_t Dim() { + return 3; + } private: @@ -116,17 +120,19 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) - { - ar & boost::serialization::make_nvp("Cal3Bundler", - boost::serialization::base_object(*this)); + void serialize(Archive & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("Cal3Bundler", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(f_); ar & BOOST_SERIALIZATION_NVP(k1_); ar & BOOST_SERIALIZATION_NVP(k2_); + ar & BOOST_SERIALIZATION_NVP(u0_); + ar & BOOST_SERIALIZATION_NVP(v0_); } /// @} -}; + }; -} // namespace gtsam + } // namespace gtsam diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index 68f482cbd..013cfe5e4 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -25,7 +25,7 @@ using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Cal3Bundler) GTSAM_CONCEPT_MANIFOLD_INST(Cal3Bundler) -static Cal3Bundler K(500, 1e-3, 1e-3); +static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000); static Point2 p(2,3); /* ************************************************************************* */ @@ -34,11 +34,12 @@ 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 expected (1000+g*p.x(), 2000+g*p.y()) ; + Point2 actual = K.uncalibrate(p); + CHECK(assert_equal(actual,expected)); } +/* ************************************************************************* */ Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); } /* ************************************************************************* */ @@ -46,7 +47,7 @@ TEST( Cal3Bundler, Duncalibrate) { Matrix Dcal, Dp; Point2 actual = K.uncalibrate(p, Dcal, Dp); - Point2 expected(1182, 1773); + Point2 expected(2182, 3773); CHECK(assert_equal(expected,actual,1e-7)); Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p); Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p); @@ -68,7 +69,7 @@ TEST( Cal3Bundler, assert_equal) /* ************************************************************************* */ TEST( Cal3Bundler, retract) { - Cal3Bundler expected(510, 2e-3, 2e-3); + Cal3Bundler expected(510, 2e-3, 2e-3, 1000, 2000); Vector d(3); d << 10, 1e-3, 1e-3; Cal3Bundler actual = K.retract(d); diff --git a/gtsam/geometry/tests/timePinholeCamera.cpp b/gtsam/geometry/tests/timePinholeCamera.cpp index abf375308..02a711ad9 100644 --- a/gtsam/geometry/tests/timePinholeCamera.cpp +++ b/gtsam/geometry/tests/timePinholeCamera.cpp @@ -52,7 +52,7 @@ int main() // Cal3DS2: 0.14201 musecs/call // After Cal3DS2 fix: 0.12231 musecs/call // Cal3Bundler: 0.12000 musecs/call - // Cal3Bundler fix: 0.13864 musecs/call + // Cal3Bundler fix: 0.14637 musecs/call { long timeLog = clock(); for(int i = 0; i < n; i++) @@ -64,12 +64,12 @@ int main() } // Oct 12 2013, iMac 3.06GHz Core i3 - // Original: 3.87199 musecs/call - // After collapse: 2.62684 musecs/call - // Cal3DS2: 4.33297 musecs/call - // After Cal3DS2 fix: 3.28565 musecs/call - // Cal3Bundler: 2.65559 musecs/call - // Cal3Bundler fix: 2.18481 musecs/call + // Original: 3.8720 musecs/call + // After collapse: 2.6269 musecs/call + // Cal3DS2: 4.3330 musecs/call + // After Cal3DS2 fix: 3.2857 musecs/call + // Cal3Bundler: 2.6556 musecs/call + // Cal3Bundler fix: 2.1613 musecs/call { Matrix Dpose, Dpoint; long timeLog = clock(); @@ -84,10 +84,10 @@ int main() // Oct 12 2013, iMac 3.06GHz Core i3 // Original: 4.0119 musecs/call // After collapse: 2.5698 musecs/call - // Cal3DS2: 4.83248 musecs/call - // After Cal3DS2 fix: 3.44832 musecs/call - // Cal3Bundler: 2.51124 musecs/call - // Cal3Bundler fix: 2.17292 musecs/call + // Cal3DS2: 4.8325 musecs/call + // After Cal3DS2 fix: 3.4483 musecs/call + // Cal3Bundler: 2.5112 musecs/call + // Cal3Bundler fix: 2.0946 musecs/call { Matrix Dpose, Dpoint, Dcal; long timeLog = clock();