some bug fixed, still not pass unit test: calibrate() converge so slow, and Jacobians are buggy

gtsam/geometry/Cal3Unify.cpp

@@ -142,24 +142,24 @@ Point2 Cal3Unify::calibrate(const Point2& pi, const double tol) const {
   // pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t})
 
   // point on the normalized plane, input for DS2
-  Point2 pnpl = this->imageToNPlane(pi);
+  double px = pi.x();
-  double px = pnpl.x();
+  double py = pi.y();
-  double py = pnpl.y();
   const Point2 invKPi ((1 / fx_) * (px - u0_ - (s_ / fy_) * (py - v0_)),
                        (1 / fy_) * (py - v0_));
 
   // initialize by ignoring the distortion at all, might be problematic for pixels around boundary
-  Point2 pn = invKPi;
+  Point2 pn = nPlaneToSpace(invKPi);
 
   // iterate until the uncalibrate is close to the actual pixel coordinate
-  const int maxIterations = 10;
+  const int maxIterations = 100;
   int iteration;
   for ( iteration = 0; iteration < maxIterations; ++iteration ) {
 
-    if ( uncalibrate(pn).distance(pi) <= tol ) break;
+    pn.print();
+    if ( pn.distance(pi) <= tol ) break;
 
-    // part1: image -> normalized plane
+    // part1: 3D space -> normalized plane
-    pnpl = this->imageToNPlane(pn);
+    Point2 pnpl = spaceToNPlane(pn);
     // part2: normalized plane -> 3D space
     px = pnpl.x(), py = pnpl.y();
     const double xy = px*py, xx = px*px, yy = py*py;
@@ -167,16 +167,16 @@ Point2 Cal3Unify::calibrate(const Point2& pi, const double tol) const {
     const double g = (1+k1_*rr+k2_*rr*rr);
     const double dx = 2*k3_*xy + k4_*(rr+2*xx);
     const double dy = 2*k4_*xy + k3_*(rr+2*yy);
-    pn = (invKPi - Point2(dx,dy))/g;
+    pn = nPlaneToSpace((invKPi - Point2(dx,dy))/g);
   }
 
   if ( iteration >= maxIterations )
-    throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization");
+    throw std::runtime_error("Cal3Unify::calibrate fails to converge. need a better initialization");
 
   return pn;
 }
 /* ************************************************************************* */
-Point2 Cal3Unify::imageToNPlane(const Point2& p) const {
+Point2 Cal3Unify::nPlaneToSpace(const Point2& p) const {
 
   const double x = p.x(), y = p.y();
   const double xy2 = x * x + y * y;
@@ -185,6 +185,15 @@ Point2 Cal3Unify::imageToNPlane(const Point2& p) const {
   return Point2((sq_xy * x / (sq_xy - xi_)), (sq_xy * y / (sq_xy - xi_)));
 }
 
+/* ************************************************************************* */
+Point2 Cal3Unify::spaceToNPlane(const Point2& p) const {
+
+  const double x = p.x(), y = p.y();
+  const double sq_xy = 1 + xi_ * sqrt(x * x + y * y + 1);
+
+  return Point2((x / sq_xy), (y / sq_xy));
+}
+
 /* ************************************************************************* */
 Cal3Unify Cal3Unify::retract(const Vector& d) const {
   return Cal3Unify(vector() + d);

gtsam/geometry/Cal3Unify.h

@@ -115,8 +115,11 @@ public:
   /// Conver a pixel coordinate to ideal coordinate
   Point2 calibrate(const Point2& p, const double tol=1e-5) const;
 
-  /// Convert a image point to normalized unit plane
+  /// Convert a 3D point to normalized unit plane
-  Point2 imageToNPlane(const Point2& p) const;
+  Point2 spaceToNPlane(const Point2& p) const;
+
+  /// Convert a normalized unit plane point to 3D space
+  Point2 nPlaneToSpace(const Point2& p) const;
 
   /// @}
   /// @name Manifold

gtsam/geometry/tests/testCal3Unify.cpp

@@ -19,16 +19,20 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Cal3Unify.h>
 
-using namespace gtsam;
 
+#include <iostream>
+using namespace gtsam;
+using namespace std;
 GTSAM_CONCEPT_TESTABLE_INST(Cal3Unify)
 GTSAM_CONCEPT_MANIFOLD_INST(Cal3Unify)
 
-/* ground truth data will from matlab, code :
+/*
+ground truth from matlab, code :
 X = [2 3 1]';
 V = [0.01, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240];
 [P, J] = spaceToImgPlane(X, V);
-matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox */
+matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox
+*/
 
 static Cal3Unify K(0.01, 100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3);
 static Point2 p(2,3);
@@ -41,10 +45,18 @@ TEST( Cal3Unify, uncalibrate)
   CHECK(assert_equal(q,p_i));
 }
 
-TEST( Cal3Unify, calibrate )
+/* ************************************************************************* */
+TEST( Cal3Unify, spaceNplane)
+{
+  Point2 q = K.spaceToNPlane(p);
+  CHECK(assert_equal(p, K.nPlaneToSpace(q)));
+}
+
+/* ************************************************************************* */
+TEST( Cal3Unify, calibrate)
 {
   Point2 pi = K.uncalibrate(p);
-  Point2 pn_hat = K.calibrate(pi);
+  Point2 pn_hat = K.calibrate(pi, 1);
   CHECK( p.equals(pn_hat, 1e-5));
 }