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

gtsam/geometry/Cal3Bundler.cpp

@@ -96,23 +96,27 @@ 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
-  double x = (pi.x() - u0_) / fx_, y = (pi.y() - v0_) / fx_;
+  double px = (pi.x() - u0_) / fx_, py = (pi.y() - v0_) / fx_;
-  const Point2 invKPi(x, y);
+  const Point2 invKPi(px, py);
-
+  Point2 pn;
-  // initialize by ignoring the distortion at all, might be problematic for
-  // pixels around boundary
-  Point2 pn(x, y);
 
   // iterate until the uncalibrate is close to the actual pixel coordinate
   const int maxIterations = 10;
-  int iteration;
+  int iteration = 0;
-  for (iteration = 0; iteration < maxIterations; ++iteration) {
+  do {
-    if (distance2(uncalibrate(pn), pi) <= tol_) break;
+    // initialize pn with distortion included
-    const double px = pn.x(), py = pn.y(), xx = px * px, yy = py * py;
+    const double rr = (px * px) + (py * py);
-    const double rr = xx + yy;
     const double g = (1 + k1_ * rr + k2_ * rr * rr);
     pn = invKPi / g;
-  }
+
+    if (distance2(uncalibrate(pn), pi) <= tol_) break;
+
+    // Set px and py using intrinsic coordinates since that is where radial
+    // distortion correction is done.
+    px = pn.x(), py = pn.y();
+    iteration++;
+
+  } while (iteration < maxIterations);
 
   if (iteration >= maxIterations)
     throw std::runtime_error(
@@ -148,4 +152,4 @@ Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
   return H;
 }
 
-}  // \ namespace gtsam
+}  // namespace gtsam

gtsam/geometry/tests/testCal3Bundler.cpp

@@ -62,6 +62,60 @@ Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) {
   return k.calibrate(pt);
 }
 
+/* ************************************************************************* */
+TEST(Cal3Bundler, DuncalibrateDefault) {
+  Cal3Bundler trueK(1, 0, 0);
+  Matrix Dcal, Dp;
+  Point2 actual = trueK.uncalibrate(p, Dcal, Dp);
+  Point2 expected = p;
+  CHECK(assert_equal(expected, actual, 1e-7));
+  Matrix numerical1 = numericalDerivative21(uncalibrate_, trueK, p);
+  Matrix numerical2 = numericalDerivative22(uncalibrate_, trueK, p);
+  CHECK(assert_equal(numerical1, Dcal, 1e-7));
+  CHECK(assert_equal(numerical2, Dp, 1e-7));
+}
+
+/* ************************************************************************* */
+TEST(Cal3Bundler, DcalibrateDefault) {
+  Cal3Bundler trueK(1, 0, 0);
+  Matrix Dcal, Dp;
+  Point2 pn(0.5, 0.5);
+  Point2 pi = trueK.uncalibrate(pn);
+  Point2 actual = trueK.calibrate(pi, Dcal, Dp);
+  CHECK(assert_equal(pn, actual, 1e-7));
+  Matrix numerical1 = numericalDerivative21(calibrate_, trueK, pi);
+  Matrix numerical2 = numericalDerivative22(calibrate_, trueK, pi);
+  CHECK(assert_equal(numerical1, Dcal, 1e-5));
+  CHECK(assert_equal(numerical2, Dp, 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(Cal3Bundler, DuncalibratePrincipalPoint) {
+  Cal3Bundler K(5, 0, 0, 2, 2);
+  Matrix Dcal, Dp;
+  Point2 actual = K.uncalibrate(p, Dcal, Dp);
+  Point2 expected(12, 17);
+  CHECK(assert_equal(expected, actual, 1e-7));
+  Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
+  Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
+  CHECK(assert_equal(numerical1, Dcal, 1e-7));
+  CHECK(assert_equal(numerical2, Dp, 1e-7));
+}
+
+/* ************************************************************************* */
+TEST(Cal3Bundler, DcalibratePrincipalPoint) {
+  Cal3Bundler K(2, 0, 0, 2, 2);
+  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));
+}
+
 /* ************************************************************************* */
 TEST(Cal3Bundler, Duncalibrate) {
   Matrix Dcal, Dp;