diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index b198643b0..36e7bf62d 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -124,8 +124,8 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, // We make use of the Implicit Function Theorem to compute the Jacobians from uncalibrate // Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can easily compute the Jacobians // df/pi = -I (pn and pi are independent args) - // Dcal = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn) - // Dp = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K + // Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn) + // Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K Matrix23 H_uncal_K; Matrix22 H_uncal_pn, H_uncal_pn_inv; diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 7fd453d45..e66c3d124 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -44,8 +44,8 @@ public: Cal3DS2() : Base() {} Cal3DS2(double fx, double fy, double s, double u0, double v0, - double k1, double k2, double p1 = 0.0, double p2 = 0.0) : - Base(fx, fy, s, u0, v0, k1, k2, p1, p2) {} + double k1, double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5) : + Base(fx, fy, s, u0, v0, k1, k2, p1, p2, tol) {} virtual ~Cal3DS2() {} diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp index 6c03883ce..c5ef117a7 100644 --- a/gtsam/geometry/Cal3DS2_Base.cpp +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -13,6 +13,7 @@ * @file Cal3DS2_Base.cpp * @date Feb 28, 2010 * @author ydjian + * @author Varun Agrawal */ #include @@ -24,8 +25,17 @@ namespace gtsam { /* ************************************************************************* */ -Cal3DS2_Base::Cal3DS2_Base(const Vector &v): - fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){} +Cal3DS2_Base::Cal3DS2_Base(const Vector& v) + : fx_(v(0)), + fy_(v(1)), + s_(v(2)), + u0_(v(3)), + v0_(v(4)), + k1_(v(5)), + k2_(v(6)), + p1_(v(7)), + p2_(v(8)), + tol_(1e-5) {} /* ************************************************************************* */ Matrix3 Cal3DS2_Base::K() const { @@ -94,9 +104,8 @@ static Matrix2 D2dintrinsic(double x, double y, double rr, } /* ************************************************************************* */ -Point2 Cal3DS2_Base::uncalibrate(const Point2& p, - OptionalJacobian<2,9> H1, OptionalJacobian<2,2> H2) const { - +Point2 Cal3DS2_Base::uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal, + OptionalJacobian<2, 2> Dp) const { // rr = x^2 + y^2; // g = (1 + k(1)*rr + k(2)*rr^2); // dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)]; @@ -115,37 +124,44 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, const double pny = g * y + dy; Matrix2 DK; - if (H1 || H2) DK << fx_, s_, 0.0, fy_; + if (Dcal || Dp) { + DK << fx_, s_, 0.0, fy_; + } // Derivative for calibration - if (H1) - *H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); + if (Dcal) { + *Dcal = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); + } // Derivative for points - if (H2) - *H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); + if (Dp) { + *Dp = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); + } // Regular uncalibrate after distortion return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_); } /* ************************************************************************* */ -Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const { +Point2 Cal3DS2_Base::calibrate(const Point2& pi, OptionalJacobian<2, 9> Dcal, + OptionalJacobian<2, 2> Dp) const { // Use the following fixed point iteration to invert the radial distortion. // pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t}) - const Point2 invKPi ((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)), - (1 / fy_) * (pi.y() - v0_)); + const Point2 invKPi((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)), + (1 / fy_) * (pi.y() - v0_)); - // initialize by ignoring the distortion at all, might be problematic for pixels around boundary + // initialize by ignoring the distortion at all, might be problematic for + // pixels around boundary Point2 pn = invKPi; // iterate until the uncalibrate is close to the actual pixel coordinate const int maxIterations = 10; int iteration; for (iteration = 0; iteration < maxIterations; ++iteration) { - if (distance2(uncalibrate(pn), pi) <= tol) break; - const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y; + if (distance2(uncalibrate(pn), pi) <= tol_) break; + const double px = pn.x(), py = pn.y(), xy = px * py, xx = px * px, + yy = py * py; const double rr = xx + yy; const double g = (1 + k1_ * rr + k2_ * rr * rr); const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx); @@ -153,8 +169,28 @@ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const { pn = (invKPi - Point2(dx, dy)) / g; } - if ( iteration >= maxIterations ) - throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization"); + if (iteration >= maxIterations) + throw std::runtime_error( + "Cal3DS2::calibrate fails to converge. need a better initialization"); + + // We make use of the Implicit Function Theorem to compute the Jacobians from uncalibrate + // Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can easily compute the Jacobians + // df/pi = -I (pn and pi are independent args) + // Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn) + // Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K + Matrix29 H_uncal_K; + Matrix22 H_uncal_pn, H_uncal_pn_inv; + + if (Dcal || Dp) { + // Compute uncalibrate Jacobians + uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn); + + H_uncal_pn_inv = H_uncal_pn.inverse(); + + if (Dp) *Dp = H_uncal_pn_inv; + if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K; + + } return pn; } diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index a0ece8bdb..b6d27cda1 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -14,6 +14,7 @@ * @brief Calibration of a camera with radial distortion * @date Feb 28, 2010 * @author ydjian + * @author Varun Agrawal */ #pragma once @@ -43,18 +44,38 @@ protected: double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point double k1_, k2_ ; // radial 2nd-order and 4th-order double p1_, p2_ ; // tangential distortion + double tol_; // tolerance value when calibrating public: /// @name Standard Constructors /// @{ - /// Default Constructor with only unit focal length - Cal3DS2_Base() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {} + /// Default Constructor with only unit focal length + Cal3DS2_Base() + : fx_(1), + fy_(1), + s_(0), + u0_(0), + v0_(0), + k1_(0), + k2_(0), + p1_(0), + p2_(0), + tol_(1e-5) {} - Cal3DS2_Base(double fx, double fy, double s, double u0, double v0, - double k1, double k2, double p1 = 0.0, double p2 = 0.0) : - fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {} + Cal3DS2_Base(double fx, double fy, double s, double u0, double v0, double k1, + double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5) + : fx_(fx), + fy_(fy), + s_(s), + u0_(u0), + v0_(v0), + k1_(k1), + k2_(k2), + p1_(p1), + p2_(p2), + tol_(tol) {} virtual ~Cal3DS2_Base() {} @@ -72,7 +93,7 @@ public: virtual void print(const std::string& s = "") const; /// assert equality up to a tolerance - bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const; + bool equals(const Cal3DS2_Base& K, double tol = 1e-8) const; /// @} /// @name Standard Interface @@ -121,12 +142,12 @@ public: * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in (distorted) image coordinates */ - Point2 uncalibrate(const Point2& p, - OptionalJacobian<2,9> Dcal = boost::none, - OptionalJacobian<2,2> Dp = boost::none) const ; + Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none) const; /// Convert (distorted) image coordinates uv to intrinsic coordinates xy - Point2 calibrate(const Point2& p, const double tol=1e-5) const; + Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none) const; /// Derivative of uncalibrate wrpt intrinsic coordinates Matrix2 D2d_intrinsic(const Point2& p) const ; @@ -164,6 +185,7 @@ private: ar & BOOST_SERIALIZATION_NVP(k2_); ar & BOOST_SERIALIZATION_NVP(p1_); ar & BOOST_SERIALIZATION_NVP(p2_); + ar & BOOST_SERIALIZATION_NVP(tol_); } /// @} diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp index b1b9c3722..247e77ae1 100644 --- a/gtsam/geometry/Cal3Unified.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -13,6 +13,7 @@ * @file Cal3Unified.cpp * @date Mar 8, 2014 * @author Jing Dong + * @author Varun Agrawal */ #include @@ -54,8 +55,8 @@ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const { /* ************************************************************************* */ // todo: make a fixed sized jacobian version of this Point2 Cal3Unified::uncalibrate(const Point2& p, - OptionalJacobian<2,10> H1, - OptionalJacobian<2,2> H2) const { + OptionalJacobian<2,10> Dcal, + OptionalJacobian<2,2> Dp) const { // this part of code is modified from Cal3DS2, // since the second part of this model (after project to normalized plane) @@ -78,16 +79,16 @@ Point2 Cal3Unified::uncalibrate(const Point2& p, Point2 puncalib = Base::uncalibrate(m, H1base, H2base); // Inlined derivative for calibration - if (H1) { + if (Dcal) { // part1 Vector2 DU; DU << -xs * sqrt_nx * xi_sqrt_nx2, // -ys * sqrt_nx * xi_sqrt_nx2; - *H1 << H1base, H2base * DU; + *Dcal << H1base, H2base * DU; } // Inlined derivative for points - if (H2) { + if (Dp) { // part1 const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx; const double mid = -(xi * xs*ys) * denom; @@ -95,20 +96,41 @@ Point2 Cal3Unified::uncalibrate(const Point2& p, DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, // mid, (sqrt_nx + xi*(xs*xs + 1)) * denom; - *H2 << H2base * DU; + *Dp << H2base * DU; } return puncalib; } /* ************************************************************************* */ -Point2 Cal3Unified::calibrate(const Point2& pi, const double tol) const { - +Point2 Cal3Unified::calibrate(const Point2& pi, OptionalJacobian<2, 10> Dcal, + OptionalJacobian<2, 2> Dp) const { // calibrate point to Nplane use base class::calibrate() - Point2 pnplane = Base::calibrate(pi, tol); + Point2 pnplane = Base::calibrate(pi); // call nplane to space - return this->nPlaneToSpace(pnplane); + Point2 pn = this->nPlaneToSpace(pnplane); + + // We make use of the Implicit Function Theorem to compute the Jacobians from uncalibrate + // Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can easily compute the Jacobians + // df/pi = -I (pn and pi are independent args) + // Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn) + // Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K + Eigen::Matrix H_uncal_K; + Matrix22 H_uncal_pn, H_uncal_pn_inv; + + if (Dcal || Dp) { + // Compute uncalibrate Jacobians + uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn); + + H_uncal_pn_inv = H_uncal_pn.inverse(); + + if (Dp) *Dp = H_uncal_pn_inv; + if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K; + + } + + return pn; } /* ************************************************************************* */ Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const { diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 381405d20..6fc37b0d1 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -14,6 +14,7 @@ * @brief Unified Calibration Model, see Mei07icra for details * @date Mar 8, 2014 * @author Jing Dong + * @author Varun Agrawal */ /** @@ -99,7 +100,8 @@ public: OptionalJacobian<2,2> Dp = boost::none) const ; /// Conver a pixel coordinate to ideal coordinate - Point2 calibrate(const Point2& p, const double tol=1e-5) const; + Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none) const; /// Convert a 3D point to normalized unit plane Point2 spaceToNPlane(const Point2& p) const; diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index 416665d46..beed09883 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -74,12 +74,26 @@ TEST( Cal3DS2, Duncalibrate2) CHECK(assert_equal(numerical,separate,1e-5)); } -/* ************************************************************************* */ -TEST( Cal3DS2, assert_equal) -{ - CHECK(assert_equal(K,K,1e-5)); +Point2 calibrate_(const Cal3DS2& k, const Point2& pt) { + return k.calibrate(pt); } +/* ************************************************************************* */ +TEST( Cal3DS2, Dcalibrate) +{ + Point2 pn(0.5, 0.5); + Point2 pi = K.uncalibrate(pn); + Matrix Dcal, Dp; + K.calibrate(pi, Dcal, Dp); + Matrix numerical1 = numericalDerivative21(calibrate_, K, pi, 1e-7); + CHECK(assert_equal(numerical1, Dcal, 1e-5)); + Matrix numerical2 = numericalDerivative22(calibrate_, K, pi, 1e-7); + CHECK(assert_equal(numerical2, Dp, 1e-5)); +} + +/* ************************************************************************* */ +TEST(Cal3DS2, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); } + /* ************************************************************************* */ TEST( Cal3DS2, retract) { diff --git a/gtsam/geometry/tests/testCal3Unified.cpp b/gtsam/geometry/tests/testCal3Unified.cpp index 2c5ffd7fb..8abb6fe04 100644 --- a/gtsam/geometry/tests/testCal3Unified.cpp +++ b/gtsam/geometry/tests/testCal3Unified.cpp @@ -82,6 +82,22 @@ TEST( Cal3Unified, Duncalibrate2) CHECK(assert_equal(numerical,computed,1e-6)); } +Point2 calibrate_(const Cal3Unified& k, const Point2& pt) { + return k.calibrate(pt); +} + +/* ************************************************************************* */ +TEST( Cal3Unified, Dcalibrate) +{ + Point2 pi = K.uncalibrate(p); + Matrix Dcal, Dp; + K.calibrate(pi, Dcal, Dp); + Matrix numerical1 = numericalDerivative21(calibrate_, K, pi); + CHECK(assert_equal(numerical1,Dcal,1e-5)); + Matrix numerical2 = numericalDerivative22(calibrate_, K, pi); + CHECK(assert_equal(numerical2,Dp,1e-5)); +} + /* ************************************************************************* */ TEST( Cal3Unified, assert_equal) {