diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 0d64d0184..e986bc820 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -25,13 +25,13 @@ namespace gtsam { /* ************************************************************************* */ Cal3Bundler::Cal3Bundler() : - f_(1), k1_(0), k2_(0), u0_(0), v0_(0) { + f_(1), k1_(0), k2_(0), u0_(0), v0_(0), tol_(1e-5) { } /* ************************************************************************* */ -Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0) : - f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0) { -} +Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0, + double tol) + : f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0), tol_(tol) {} /* ************************************************************************* */ Matrix3 Cal3Bundler::K() const { @@ -94,21 +94,24 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, // } /* ************************************************************************* */ -Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const { +Point2 Cal3Bundler::calibrate(const Point2& pi, + OptionalJacobian<2, 3> Dcal, + OptionalJacobian<2, 2> Dp) const { // Copied from Cal3DS2 :-( // but specialized with k1,k2 non-zero only and fx=fy and s=0 - const Point2 invKPi((pi.x() - u0_)/f_, (pi.y() - v0_)/f_); + double x = (pi.x() - u0_)/f_, y = (pi.y() - v0_)/f_; + const Point2 invKPi(x, y); // initialize by ignoring the distortion at all, might be problematic for pixels around boundary - Point2 pn = invKPi; + Point2 pn(x, y); // 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) + if (distance2(uncalibrate(pn), pi) <= tol_) break; - const double x = pn.x(), y = pn.y(), xx = x * x, yy = y * y; + const double px = pn.x(), py = pn.y(), xx = px * px, yy = py * py; const double rr = xx + yy; const double g = (1 + k1_ * rr + k2_ * rr * rr); pn = invKPi / g; @@ -118,6 +121,22 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const { throw std::runtime_error( "Cal3Bundler::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) + // 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 + Matrix23 H_uncal_K; + Matrix22 H_uncal_pn; + + if (Dcal || Dp) { + // Compute uncalibrate Jacobians + uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn); + + if (Dp) *Dp = H_uncal_pn.inverse(); + if (Dcal) *Dcal = -H_uncal_pn.inverse() * H_uncal_K; + + } return pn; } diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 4787f565b..96765f899 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -33,6 +33,7 @@ private: double f_; ///< focal length double k1_, k2_; ///< radial distortion double u0_, v0_; ///< image center, not a parameter to be optimized but a constant + double tol_; ///< tolerance value when calibrating public: @@ -51,8 +52,10 @@ public: * @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 + * @param tol optional calibration tolerance value */ - Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0); + Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0, + double tol = 1e-5); virtual ~Cal3Bundler() {} @@ -117,8 +120,17 @@ public: Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none, OptionalJacobian<2, 2> Dp = boost::none) const; - /// Convert a pixel coordinate to ideal coordinate - Point2 calibrate(const Point2& pi, const double tol = 1e-5) const; + /** + * Convert a pixel coordinate to ideal coordinate xy + * @param p point in image coordinates + * @param tol optional tolerance threshold value for iterative minimization + * @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in intrinsic coordinates + */ + Point2 calibrate(const Point2& pi, + OptionalJacobian<2, 3> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none) const; /// @deprecated might be removed in next release, use uncalibrate Matrix2 D2d_intrinsic(const Point2& p) const; @@ -164,6 +176,7 @@ private: ar & BOOST_SERIALIZATION_NVP(k2_); ar & BOOST_SERIALIZATION_NVP(u0_); ar & BOOST_SERIALIZATION_NVP(v0_); + ar & BOOST_SERIALIZATION_NVP(tol_); } /// @} diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index 8de049fa4..448600266 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -52,12 +52,14 @@ TEST( Cal3Bundler, calibrate ) Point2 pn(0.5, 0.5); Point2 pi = K.uncalibrate(pn); Point2 pn_hat = K.calibrate(pi); - CHECK( traits::Equals(pn, pn_hat, 1e-5)); + CHECK(traits::Equals(pn, pn_hat, 1e-5)); } /* ************************************************************************* */ Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); } +Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { return k.calibrate(pt); } + /* ************************************************************************* */ TEST( Cal3Bundler, Duncalibrate) { @@ -69,11 +71,20 @@ TEST( Cal3Bundler, Duncalibrate) 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)); +} + +/* ************************************************************************* */ +TEST( Cal3Bundler, Dcalibrate) +{ + Matrix Dcal, Dp; + Point2 pn(0.5, 0.5); + Point2 pi = K.uncalibrate(pn); + Point2 actual = K.calibrate(pi, Dcal, Dp); + CHECK(assert_equal(pn, actual, 1e-7)); + Matrix numerical1 = numericalDerivative21(calibrate_, K, pi); + Matrix numerical2 = numericalDerivative22(calibrate_, K, pi); + CHECK(assert_equal(numerical1,Dcal,1e-5)); + CHECK(assert_equal(numerical2,Dp,1e-5)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index a9b68bdec..ad6f4e921 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -336,6 +336,15 @@ TEST( PinholeCamera, range3) { EXPECT(assert_equal(Hexpected2, D2, 1e-7)); } +/* ************************************************************************* */ +TEST( PinholeCamera, Cal3Bundler) { + Cal3Bundler calibration; + Pose3 wTc; + PinholeCamera camera(wTc, calibration); + Point2 p(50, 100); + camera.backproject(p, 10); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 7dbd1c6ef..a172df315 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -959,6 +959,7 @@ class Cal3Bundler { // Standard Constructors Cal3Bundler(); Cal3Bundler(double fx, double k1, double k2, double u0, double v0); + Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol); // Testable void print(string s) const; @@ -971,7 +972,7 @@ class Cal3Bundler { Vector localCoordinates(const gtsam::Cal3Bundler& c) const; // Action on Point2 - gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; + gtsam::Point2 calibrate(const gtsam::Point2& p) const; gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; // Standard Interface