Jacobians for Camera models

- Add jacobians for calibrate function using implicit function theorem
- Consistent naming of jacobian parameters
- Added tests for jacobians
- Some simple formatting
- Fixed docs for implicit function theorem
- Added parentheses to conform with Google style

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/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
-  // Dp = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
+  // 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;
 

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) :
+      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) {}
+        Base(fx, fy, s, u0, v0, k1, k2, p1, p2, tol) {}
 
   virtual ~Cal3DS2() {}
 

gtsam/geometry/Cal3DS2_Base.cpp

@@ -13,6 +13,7 @@
  * @file Cal3DS2_Base.cpp
  * @date Feb 28, 2010
  * @author ydjian
+ * @author Varun Agrawal
  */
 
 #include <gtsam/base/Vector.h>
@@ -24,8 +25,17 @@
 namespace gtsam {
 
 /* ************************************************************************* */
-Cal3DS2_Base::Cal3DS2_Base(const Vector &v):
+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]){}
+    : 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,
+Point2 Cal3DS2_Base::uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal,
-    OptionalJacobian<2,9> H1, OptionalJacobian<2,2> H2) const {
+                                 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)
+  if (Dcal) {
-    *H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
+    *Dcal = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
+  }
 
   // Derivative for points
-  if (H2)
+  if (Dp) {
-    *H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
+    *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_)),
+  const Point2 invKPi((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)),
-                       (1 / 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;
+    if (distance2(uncalibrate(pn), pi) <= tol_) break;
-    const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y;
+    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 )
+  if (iteration >= maxIterations)
-    throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization");
+    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;
 }

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
+ /// 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) {}
+ 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,
+ Cal3DS2_Base(double fx, double fy, double s, double u0, double v0, double k1,
-      double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
+              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) {}
+     : 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,
+  Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
-       OptionalJacobian<2,9> Dcal = boost::none,
+                     OptionalJacobian<2, 2> Dp = boost::none) const;
-       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_);
   }
 
   /// @}

gtsam/geometry/Cal3Unified.cpp

@@ -13,6 +13,7 @@
  * @file Cal3Unified.cpp
  * @date Mar 8, 2014
  * @author Jing Dong
+ * @author Varun Agrawal
  */
 
 #include <gtsam/base/Vector.h>
@@ -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,10> Dcal,
-       OptionalJacobian<2,2> H2) const {
+       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<double, 2, 10> 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 {

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;

gtsam/geometry/tests/testCal3DS2.cpp

@@ -74,12 +74,26 @@ TEST( Cal3DS2, Duncalibrate2)
   CHECK(assert_equal(numerical,separate,1e-5));
 }
 
-/* ************************************************************************* */
+Point2 calibrate_(const Cal3DS2& k, const Point2& pt) {
-TEST( Cal3DS2, assert_equal)
+  return k.calibrate(pt);
-{
-  CHECK(assert_equal(K,K,1e-5));
 }
 
+/* ************************************************************************* */
+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)
 {

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)
 {