Finished some re-factoring in Cal3DS2

gtsam/geometry/Cal3DS2.cpp

@@ -25,7 +25,7 @@ namespace gtsam {
 
 /* ************************************************************************* */
 Cal3DS2::Cal3DS2(const Vector &v):
-    fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), k3_(v[7]), k4_(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]){}
 
 /* ************************************************************************* */
 Matrix Cal3DS2::K() const {
@@ -34,32 +34,64 @@ Matrix Cal3DS2::K() const {
 
 /* ************************************************************************* */
 Vector Cal3DS2::vector() const {
-  return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_);
+  return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_);
 }
 
 /* ************************************************************************* */
-void Cal3DS2::print(const std::string& s) const {
+void Cal3DS2::print(const std::string& s_) const {
-  gtsam::print(K(), s + ".K");
+  gtsam::print(K(), s_ + ".K");
-  gtsam::print(Vector(k()), s + ".k");
+  gtsam::print(Vector(k()), s_ + ".k");
 }
 
 /* ************************************************************************* */
 bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
   if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol ||
       fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol ||
-      fabs(k2_ - K.k2_) > tol || fabs(k3_ - K.k3_) > tol || fabs(k4_ - K.k4_) > tol)
+      fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol)
     return false;
   return true;
 }
 
 /* ************************************************************************* */
-Point2 Cal3DS2::uncalibrate(const Point2& p,
+static Eigen::Matrix<double, 2, 9> D2dcalibration(double x, double y, double xx,
-       boost::optional<Matrix&> H1,
+    double yy, double xy, double rr, double r4, double pnx, double pny,
-       boost::optional<Matrix&> H2) const {
+    const Eigen::Matrix<double, 2, 2>& DK) {
+  Eigen::Matrix<double, 2, 5> DR1;
+  DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
+  Eigen::Matrix<double, 2, 4> DR2;
+  DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
+         y * rr, y * r4, rr + 2 * yy, 2 * xy;
+  Eigen::Matrix<double, 2, 9> D;
+  D << DR1, DK * DR2;
+  return D;
+}
 
-  // parameters
+/* ************************************************************************* */
-  const double fx = fx_, fy = fy_, s = s_;
+static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr,
-  const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_;
+    double g, double k1, double k2, double p1, double p2,
+    const Eigen::Matrix<double, 2, 2>& DK) {
+  const double drdx = 2. * x;
+  const double drdy = 2. * y;
+  const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
+  const double dgdy = k1 * drdy + k2 * 2. * rr * drdy;
+
+  // Dx = 2*p1*xy + p2*(rr+2*xx);
+  // Dy = 2*p2*xy + p1*(rr+2*yy);
+  const double dDxdx = 2. * p1 * y + p2 * (drdx + 4. * x);
+  const double dDxdy = 2. * p1 * x + p2 * drdy;
+  const double dDydx = 2. * p2 * y + p1 * drdx;
+  const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
+
+  Eigen::Matrix<double, 2, 2> DR;
+  DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
+        y * dgdx + dDydx, g + y * dgdy + dDydy;
+
+  return DK * DR;
+}
+
+/* ************************************************************************* */
+Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
+    boost::optional<Matrix&> H2) const {
 
   //  rr = x^2 + y^2;
   //  g = (1 + k(1)*rr + k(2)*rr^2);
@@ -68,40 +100,29 @@ Point2 Cal3DS2::uncalibrate(const Point2& p,
   const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y;
   const double rr = xx + yy;
   const double r4 = rr * rr;
-  const double g = 1. + k1 * rr + k2 * r4;
+  const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor
-  const double dx = 2. * k3 * xy + k4 * (rr + 2. * xx);
-  const double dy = 2. * k4 * xy + k3 * (rr + 2. * yy);
 
-  const double pnx = g*x + dx;
+  // tangential component
-  const double pny = g*y + dy;
+  const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx);
+  const double dy = 2. * p2_ * xy + p1_ * (rr + 2. * yy);
 
-  // Inlined derivative for calibration
+  // Radial and tangential distortion applied
-  if (H1) {
+  const double pnx = g * x + dx;
-    *H1 = (Matrix(2, 9) <<  pnx, 0.0, pny, 1.0, 0.0, fx * x * rr + s * y * rr,
+  const double pny = g * y + dy;
-        fx * x * r4 + s * y * r4, fx * 2. * xy + s * (rr + 2. * yy),
-        fx * (rr + 2. * xx) + s * (2. * xy), 0.0, pny, 0.0, 0.0, 1.0,
-        fy * y * rr, fy * y * r4, fy * (rr + 2. * yy), fy * (2. * xy));
-  }
-  // Inlined derivative for points
-  if (H2) {
-    const double dr_dx = 2. * x;
-    const double dr_dy = 2. * y;
-    const double dg_dx = k1 * dr_dx + k2 * 2. * rr * dr_dx;
-    const double dg_dy = k1 * dr_dy + k2 * 2. * rr * dr_dy;
 
-    const double dDx_dx = 2. * k3 * y + k4 * (dr_dx + 4. * x);
+  Eigen::Matrix<double, 2, 2> DK;
-    const double dDx_dy = 2. * k3 * x + k4 * dr_dy;
+  if (H1 || H2) DK << fx_, s_, 0.0, fy_;
-    const double dDy_dx = 2. * k4 * y + k3 * dr_dx;
-    const double dDy_dy = 2. * k4 * x + k3 * (dr_dy + 4. * y);
 
-    Matrix DK = (Matrix(2, 2) << fx, s_, 0.0, fy);
+  // Derivative for calibration
-    Matrix DR = (Matrix(2, 2) << g + x * dg_dx + dDx_dx, x * dg_dy + dDx_dy,
+  if (H1)
-        y * dg_dx + dDy_dx, g + y * dg_dy + dDy_dy);
+    *H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
 
-    *H2 = DK * DR;
+  // Derivative for points
-  }
+  if (H2)
+    *H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
 
-  return Point2(fx * pnx + s * pny + u0_, fy * pny + v0_);
+  // Regular uncalibrate after distortion
+  return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
 }
 
 /* ************************************************************************* */
@@ -118,14 +139,14 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const {
   // iterate until the uncalibrate is close to the actual pixel coordinate
   const int maxIterations = 10;
   int iteration;
-  for ( iteration = 0; iteration < maxIterations; ++iteration ) {
+  for (iteration = 0; iteration < maxIterations; ++iteration) {
-    if ( uncalibrate(pn).distance(pi) <= tol ) break;
+    if (uncalibrate(pn).distance(pi) <= tol) break;
-    const double x = pn.x(), y = pn.y(), xy = x*y, xx = x*x, yy = y*y;
+    const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y;
     const double rr = xx + yy;
-    const double g = (1+k1_*rr+k2_*rr*rr);
+    const double g = (1 + k1_ * rr + k2_ * rr * rr);
-    const double dx = 2*k3_*xy + k4_*(rr+2*xx);
+    const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
-    const double dy = 2*k4_*xy + k3_*(rr+2*yy);
+    const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
-    pn = (invKPi - Point2(dx,dy))/g;
+    pn = (invKPi - Point2(dx, dy)) / g;
   }
 
   if ( iteration >= maxIterations )
@@ -136,47 +157,28 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const {
 
 /* ************************************************************************* */
 Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
-  //const double fx = fx_, fy = fy_, s = s_;
+  const double x = p.x(), y = p.y(), xx = x * x, yy = y * y;
-  const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_;
-  //const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y;
-  const double x = p.x(), y = p.y(), xx = x*x, yy = y*y;
   const double rr = xx + yy;
-  const double dr_dx = 2*x;
+  const double r4 = rr * rr;
-  const double dr_dy = 2*y;
+  const double g = (1 + k1_ * rr + k2_ * r4);
-  const double r4 = rr*rr;
+  Eigen::Matrix<double, 2, 2> DK;
-  const double g = 1 + k1*rr + k2*r4;
+  DK << fx_, s_, 0.0, fy_;
-  const double dg_dx = k1*dr_dx + k2*2*rr*dr_dx;
+  return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
-  const double dg_dy = k1*dr_dy + k2*2*rr*dr_dy;
-
-  // Dx = 2*k3*xy + k4*(rr+2*xx);
-  // Dy = 2*k4*xy + k3*(rr+2*yy);
-  const double dDx_dx = 2*k3*y + k4*(dr_dx + 4*x);
-  const double dDx_dy = 2*k3*x + k4*dr_dy;
-  const double dDy_dx = 2*k4*y + k3*dr_dx;
-  const double dDy_dy = 2*k4*x + k3*(dr_dy + 4*y);
-
-  Matrix DK = (Matrix(2, 2) << fx_, s_, 0.0, fy_);
-  Matrix DR = (Matrix(2, 2) << g + x*dg_dx + dDx_dx,     x*dg_dy + dDx_dy,
-             y*dg_dx + dDy_dx, g + y*dg_dy + dDy_dy);
-
-  return DK * DR;
 }
 
 /* ************************************************************************* */
 Matrix Cal3DS2::D2d_calibration(const Point2& p) const {
-  const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y;
+  const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y;
   const double rr = xx + yy;
-  const double r4 = rr*rr;
+  const double r4 = rr * rr;
-  const double fx = fx_, fy = fy_, s = s_;
+  const double g = (1 + k1_ * rr + k2_ * r4);
-  const double g = (1+k1_*rr+k2_*r4);
+  const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
-  const double dx = 2*k3_*xy + k4_*(rr+2*xx);
+  const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
-  const double dy = 2*k4_*xy + k3_*(rr+2*yy);
+  const double pnx = g * x + dx;
-  const double pnx = g*x + dx;
+  const double pny = g * y + dy;
-  const double pny = g*y + dy;
+  Eigen::Matrix<double, 2, 2> DK;
-
+  DK << fx_, s_, 0.0, fy_;
-  return (Matrix(2, 9) <<
+  return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
-  pnx, 0.0, pny, 1.0, 0.0, fx*x*rr + s*y*rr, fx*x*r4 + s*y*r4, fx*2*xy + s*(rr+2*yy), fx*(rr+2*xx) + s*(2*xy),
-  0.0, pny, 0.0, 0.0, 1.0, fy*y*rr      , fy*y*r4         , fy*(rr+2*yy)         , fy*(2*xy)  );
 }
 
 /* ************************************************************************* */

gtsam/geometry/Cal3DS2.h

@@ -27,6 +27,15 @@ namespace gtsam {
  * @brief Calibration of a camera with radial distortion
  * @addtogroup geometry
  * \nosubgrouping
+ *
+ * Uses same distortionmodel as OpenCV, with
+ * http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
+ * but using only k1,k2,p1, and p2 coefficients.
+ * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
+ * rr = Pn.x^2 + Pn.y^2
+ * \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
+ *                      k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y  ]
+ * pi = K*pn
  */
 class GTSAM_EXPORT Cal3DS2 : public DerivedValue<Cal3DS2> {
 
@@ -34,28 +43,22 @@ protected:
 
   double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
   double k1_, k2_ ; // radial 2nd-order and 4th-order
-  double k3_, k4_ ; // tangential distortion
+  double p1_, p2_ ; // tangential distortion
-
-  // K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
-  // rr = Pn.x^2 + Pn.y^2
-  // \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
-  //                      k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y  ]
-  // pi = K*pn
 
 public:
   Matrix K() const ;
-  Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, k3_, k4_); }
+  Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); }
   Vector vector() const ;
 
   /// @name Standard Constructors
   /// @{
 
   /// Default Constructor with only unit focal length
-  Cal3DS2() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {}
+  Cal3DS2() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {}
 
   Cal3DS2(double fx, double fy, double s, double u0, double v0,
-      double k1, double k2, double k3 = 0.0, double k4 = 0.0) :
+      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), k3_(k3), k4_(k4) {}
+  fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
 
   /// @}
   /// @name Advanced Constructors
@@ -92,18 +95,30 @@ public:
   /// image center in y
   inline double py() const { return v0_;}
 
+  /// First distortion coefficient
+  inline double k1() const { return k1_;}
+
+  /// Second distortion coefficient
+  inline double k2() const { return k2_;}
+
+  /// First tangential distortion coefficient
+  inline double p1() const { return p1_;}
+
+  /// Second tangential distortion coefficient
+  inline double p2() const { return p2_;}
+
   /**
-   * convert intrinsic coordinates xy to image coordinates uv
+   * convert intrinsic coordinates xy to (distorted) image coordinates uv
    * @param p point in intrinsic coordinates
    * @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
-   * @return point in image coordinates
+   * @return point in (distorted) image coordinates
    */
   Point2 uncalibrate(const Point2& p,
       boost::optional<Matrix&> Dcal = boost::none,
       boost::optional<Matrix&> Dp = boost::none) const ;
 
-  /// Conver a pixel coordinate to ideal coordinate
+  /// Convert (distorted) image coordinates uv to intrinsic coordinates xy
   Point2 calibrate(const Point2& p, const double tol=1e-5) const;
 
   /// Derivative of uncalibrate wrpt intrinsic coordinates
@@ -148,8 +163,8 @@ private:
     ar & BOOST_SERIALIZATION_NVP(v0_);
     ar & BOOST_SERIALIZATION_NVP(k1_);
     ar & BOOST_SERIALIZATION_NVP(k2_);
-    ar & BOOST_SERIALIZATION_NVP(k3_);
+    ar & BOOST_SERIALIZATION_NVP(p1_);
-    ar & BOOST_SERIALIZATION_NVP(k4_);
+    ar & BOOST_SERIALIZATION_NVP(p2_);
   }
 
 

gtsam/geometry/Cal3Unified.cpp

@@ -43,7 +43,7 @@ void Cal3Unified::print(const std::string& s) const {
 bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
   if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol ||
       fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol ||
-      fabs(k2_ - K.k2_) > tol || fabs(k3_ - K.k3_) > tol || fabs(k4_ - K.k4_) > tol ||
+      fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol ||
       fabs(xi_ - K.xi_) > tol)
     return false;
   return true;

gtsam/geometry/Cal3Unified.h

@@ -31,6 +31,14 @@ namespace gtsam {
  * @brief Calibration of a omni-directional camera with mirror + lens radial distortion
  * @addtogroup geometry
  * \nosubgrouping
+ *
+ * Similar to Cal3DS2, does distortion but has additional mirror parameter xi
+ * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
+ * Pn = [ P.x / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})]
+ * rr = Pn.x^2 + Pn.y^2
+ * \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
+ *                      k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y  ]
+ * pi = K*pn
  */
 class GTSAM_EXPORT Cal3Unified : public Cal3DS2 {
 
@@ -41,13 +49,6 @@ private:
 
   double xi_;  // mirror parameter
 
-  // K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
-  // Pn = [ P.x / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})]
-  // rr = Pn.x^2 + Pn.y^2
-  // \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
-  //                      k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y  ]
-  // pi = K*pn
-
 public:
   //Matrix K() const ;
   //Eigen::Vector4d k() const { return Base::k(); }
@@ -60,8 +61,8 @@ public:
   Cal3Unified() : Base(), xi_(0) {}
 
   Cal3Unified(double fx, double fy, double s, double u0, double v0,
-      double k1, double k2, double k3 = 0.0, double k4 = 0.0, double xi = 0.0) :
+      double k1, double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0) :
-        Base(fx, fy, s, u0, v0, k1, k2, k3, k4), xi_(xi) {}
+        Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {}
 
   /// @}
   /// @name Advanced Constructors

gtsam/geometry/tests/testCal3DS2.cpp

@@ -60,6 +60,8 @@ TEST( Cal3DS2, Duncalibrate1)
   K.uncalibrate(p, computed, boost::none);
   Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
   CHECK(assert_equal(numerical,computed,1e-5));
+  Matrix separate = K.D2d_calibration(p);
+  CHECK(assert_equal(numerical,separate,1e-5));
 }
 
 /* ************************************************************************* */
@@ -68,6 +70,8 @@ TEST( Cal3DS2, Duncalibrate2)
   Matrix computed; K.uncalibrate(p, boost::none, computed);
   Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
   CHECK(assert_equal(numerical,computed,1e-5));
+  Matrix separate = K.D2d_intrinsic(p);
+  CHECK(assert_equal(numerical,separate,1e-5));
 }
 
 /* ************************************************************************* */