Merge pull request #539 from borglab/fix/cal3bundler-jacobians

Add calibrate with jacobians for Cal3Bundler
borglab · Sep 29, 2020 · 1bbd233 · 1bbd233
2 parents 6fdf39d + 7dab718
commit 1bbd233
Show file tree

Hide file tree

Showing 5 changed files with 72 additions and 19 deletions.
diff --git 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
@@ -33,6 +33,7 @@ class GTSAM_EXPORT Cal3Bundler {
   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 @@ class GTSAM_EXPORT Cal3Bundler {
    *  @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 @@ class GTSAM_EXPORT Cal3Bundler {
   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 @@ class GTSAM_EXPORT Cal3Bundler {
     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
@@ -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<Point2>::Equals(pn, pn_hat, 1e-5));
+  CHECK(traits<Point2>::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
@@ -336,6 +336,15 @@ TEST( PinholeCamera, range3) {
   EXPECT(assert_equal(Hexpected2, D2, 1e-7));
 }
 
+/* ************************************************************************* */
+TEST( PinholeCamera, Cal3Bundler) {
+  Cal3Bundler calibration;
+  Pose3 wTc;
+  PinholeCamera<Cal3Bundler> 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
@@ -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