Merged in feature/addJacobian_Cal3_S2 (pull request #162)

Add optional Jacobian to calibrate for Cal3_D2 camera instrinsics

gtsam/geometry/Cal3_S2.cpp

@@ -83,10 +83,21 @@ Point2 Cal3_S2::uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal,
 }
 
 /* ************************************************************************* */
-Point2 Cal3_S2::calibrate(const Point2& p) const {
+Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2,5> Dcal,
-  const double u = p.x(), v = p.y();
+                           OptionalJacobian<2,2> Dp) const {
-  return Point2((1 / fx_) * (u - u0_ - (s_ / fy_) * (v - v0_)),
+    const double u = p.x(), v = p.y();
-      (1 / fy_) * (v - v0_));
+    double delta_u = u - u0_, delta_v = v - v0_;
+    double inv_fx = 1/ fx_, inv_fy = 1/fy_;
+    double inv_fy_delta_v = inv_fy * delta_v, inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
+    Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v),
+                  inv_fy_delta_v);
+    if(Dcal)
+       *Dcal << - inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v,  -inv_fx * point.y(),
+            -inv_fx,  inv_fx_s_inv_fy,
+            0, -inv_fy * point.y(), 0,  0, -inv_fy;
+    if(Dp)
+        *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy;
+    return point;
 }
 
 /* ************************************************************************* */

gtsam/geometry/Cal3_S2.h

@@ -156,9 +156,12 @@ public:
   /**
    * convert image coordinates uv to intrinsic coordinates xy
    * @param p point in image coordinates
+   * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in intrinsic coordinates
    */
-  Point2 calibrate(const Point2& p) const;
+  Point2 calibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none,
+                   OptionalJacobian<2,2> Dp = boost::none) const;
 
   /**
    * convert homogeneous image coordinates to intrinsic coordinates

gtsam/geometry/tests/testCal3_S2.cpp

@@ -26,6 +26,8 @@ GTSAM_CONCEPT_MANIFOLD_INST(Cal3_S2)
 
 static Cal3_S2 K(500, 500, 0.1, 640 / 2, 480 / 2);
 static Point2 p(1, -2);
+static Point2 p_uv(1320.3, 1740);
+static Point2 p_xy(2, 3);
 
 /* ************************************************************************* */
 TEST( Cal3_S2, easy_constructor)
@@ -73,6 +75,27 @@ TEST( Cal3_S2, Duncalibrate2)
   CHECK(assert_equal(numerical,computed,1e-9));
 }
 
+Point2 calibrate_(const Cal3_S2& k,  const Point2& pt) {return k.calibrate(pt); }
+/* ************************************************************************* */
+TEST(Cal3_S2, Dcalibrate1)
+{
+    Matrix computed;
+    Point2 expected = K.calibrate(p_uv, computed, boost::none);
+    Matrix numerical = numericalDerivative21(calibrate_, K, p_uv);
+    CHECK(assert_equal(expected, p_xy, 1e-8));
+    CHECK(assert_equal(numerical, computed, 1e-8));
+}
+
+/* ************************************************************************* */
+TEST(Cal3_S2, Dcalibrate2)
+{
+    Matrix computed;
+    Point2 expected = K.calibrate(p_uv, boost::none, computed);
+    Matrix numerical = numericalDerivative22(calibrate_, K, p_uv);
+    CHECK(assert_equal(expected, p_xy, 1e-8));
+    CHECK(assert_equal(numerical, computed, 1e-8));
+}
+
 /* ************************************************************************* */
 TEST( Cal3_S2, assert_equal)
 {