gtsam/geometry/Cal3Bundler.cpp

@@ -94,6 +94,34 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
   return Point2(u0_ + f_ * u, v0_ + f_ * v);
 }
 
+/* ************************************************************************* */
+Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) 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_);
+
+  // 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 (uncalibrate(pn).distance(pi) <= tol)
+      break;
+    const double x = pn.x(), y = pn.y(), xx = x * x, yy = y * y;
+    const double rr = xx + yy;
+    const double g = (1 + k1_ * rr + k2_ * rr * rr);
+    pn = invKPi / g;
+  }
+
+  if (iteration >= maxIterations)
+    throw std::runtime_error(
+        "Cal3DS2::calibrate fails to converge. need a better initialization");
+
+  return pn;
+}
+
 /* ************************************************************************* */
 Matrix Cal3Bundler::D2d_intrinsic(const Point2& p) const {
   Matrix Dp;

gtsam/geometry/Cal3Bundler.h

@@ -92,7 +92,10 @@ public:
   Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal =
       boost::none, boost::optional<Matrix&> Dp = boost::none) const;
 
-  /// @deprecated might be removed in next release, use uncalibrate
+  /// Conver a pixel coordinate to ideal coordinate
+  Point2 calibrate(const Point2& pi, const double tol=1e-5) const;
+
+    /// @deprecated might be removed in next release, use uncalibrate
   Matrix D2d_intrinsic(const Point2& p) const;
 
   /// @deprecated might be removed in next release, use uncalibrate

gtsam/geometry/tests/testCal3Bundler.cpp

@@ -29,7 +29,7 @@ static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000);
 static Point2 p(2,3);
 
 /* ************************************************************************* */
-TEST( Cal3Bundler, calibrate)
+TEST( Cal3Bundler, uncalibrate)
 {
   Vector v = K.vector() ;
   double r = p.x()*p.x() + p.y()*p.y() ;
@@ -39,6 +39,14 @@ TEST( Cal3Bundler, calibrate)
   CHECK(assert_equal(actual,expected));
 }
 
+TEST( Cal3Bundler, calibrate )
+{
+  Point2 pn(0.5, 0.5);
+  Point2 pi = K.uncalibrate(pn);
+  Point2 pn_hat = K.calibrate(pi);
+  CHECK( pn.equals(pn_hat, 1e-5));
+}
+
 /* ************************************************************************* */
 Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); }