From f7bf418c45f7bc7bb17207b798e4fae087164454 Mon Sep 17 00:00:00 2001
From: lvzhaoyang <lvzhaoyang1990@gmail.com>
Date: Tue, 23 Jun 2015 01:46:50 -0400
Subject: [PATCH] feature: add jacobian to Cal3_D2 calibrate()

---
 gtsam/geometry/Cal3_S2.cpp | 18 ++++++++++++++++++
 gtsam/geometry/Cal3_S2.h   | 10 ++++++++++
 2 files changed, 28 insertions(+)

diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp
index 3ec29bbd2..81123d9a9 100644
--- a/gtsam/geometry/Cal3_S2.cpp
+++ b/gtsam/geometry/Cal3_S2.cpp
@@ -89,6 +89,24 @@ Point2 Cal3_S2::calibrate(const Point2& p) const {
       (1 / fy_) * (v - v0_));
 }
 
+/* ************************************************************************* */
+Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none,
+                          OptionalJacobian<2,2> Dp = boost::none) const {
+    const double u = p.x(), v = p.y();
+    double delta_u = u - u0_, delta_v = v - v0_;
+    double inv_fx = 1/ fx_, inv_fy = 1/fy_;
+    if(Dcal)
+       *Dcal << - inv_fx * inv_fx * (delta_u - s_  * inv_fy * delta_v)
+             << inv_fx * (s_ * inv_fy * inv_fy) * delta_v << -inv_fx * inv_fy * delta_v
+             << -inv_fx << inv_fx * s_ * inv_fy
+             << 0 << -inv_fy * inv_fy * delta_v << 0 << 0 << -inv_fy;
+    if(Dp)
+        *Dp << inv_fx << -inv_fx * s_ * inv_fy << 0 << inv_fy;
+    return Point2(inv_fx * (delta_u - s_ * inv_fy * delta_v),
+        inv_fy * delta_v);
+}
+
+
 /* ************************************************************************* */
 Vector3 Cal3_S2::calibrate(const Vector3& p) const {
   return matrix_inverse() * p;
diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h
index 3d5342c92..c448cb0b1 100644
--- a/gtsam/geometry/Cal3_S2.h
+++ b/gtsam/geometry/Cal3_S2.h
@@ -160,6 +160,16 @@ public:
    */
   Point2 calibrate(const Point2& p) const;
 
+  /**
+   * 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, OptionalJacobian<2,5> Dcal = boost::none,
+                   OptionalJacobian<2,2> Dp = boost::none) const;
+
   /**
    * convert homogeneous image coordinates to intrinsic coordinates
    * @param p point in image coordinates