From c9936763e29530fce51487656815d9a06c87778e Mon Sep 17 00:00:00 2001
From: nsrinivasan7 <natesh.rvce@gmail.com>
Date: Thu, 27 Nov 2014 10:34:44 -0500
Subject: [PATCH] testCal3S2

---
 gtsam/geometry/Cal3_S2.cpp           | 15 ---------------
 gtsam/geometry/Cal3_S2.h             | 10 ----------
 gtsam/geometry/PinholeCamera.h       |  8 ++++----
 gtsam/geometry/tests/testCal3_S2.cpp |  2 +-
 gtsam/slam/SmartFactorBase.h         |  2 +-
 5 files changed, 6 insertions(+), 31 deletions(-)

diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp
index aece1ded1..ff5a42b10 100644
--- a/gtsam/geometry/Cal3_S2.cpp
+++ b/gtsam/geometry/Cal3_S2.cpp
@@ -71,21 +71,6 @@ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
   return true;
 }
 
-/* ************************************************************************* */
-Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
-    boost::optional<Matrix&> Dp) const {
-  const double x = p.x(), y = p.y();
-  if (Dcal) {
-    Dcal->resize(2,5);
-    *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
-  }
-  if (Dp) {
-    Dp->resize(2,2);
-    *Dp << fx_, s_, 0.0, fy_;
-  }
-  return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
-}
-
 /* ************************************************************************* */
 Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix25&> Dcal,
     boost::optional<Matrix2&> Dp) const {
diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h
index 6317d251d..67f0d7e41 100644
--- a/gtsam/geometry/Cal3_S2.h
+++ b/gtsam/geometry/Cal3_S2.h
@@ -158,16 +158,6 @@ public:
   Point2 uncalibrate(const Point2& p, boost::optional<Matrix25&> Dcal,
       boost::optional<Matrix2&> Dp) const;
 
-  /**
-   * convert intrinsic coordinates xy to image coordinates uv, dynamic derivaitves
-   * @param p point in intrinsic coordinates
-   * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
-   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
-   * @return point in image coordinates
-   */
-  Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
-      boost::optional<Matrix&> Dp) const;
-
   /**
    * convert image coordinates uv to intrinsic coordinates xy
    * @param p point in image coordinates
diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h
index c5174ae65..7e61c2b53 100644
--- a/gtsam/geometry/PinholeCamera.h
+++ b/gtsam/geometry/PinholeCamera.h
@@ -353,7 +353,7 @@ public:
       const Point3& pw, //
       boost::optional<Matrix&> Dpose,
       boost::optional<Matrix&> Dpoint,
-      boost::optional<Matrix&> Dcal) const {
+      boost::optional<Matrix25&> Dcal) const {
 
     // Transform to camera coordinates and check cheirality
     const Point3 pc = pose_.transform_to(pw);
@@ -365,7 +365,7 @@ public:
       const double z = pc.z(), d = 1.0 / z;
 
       // uncalibration
-      Matrix Dpi_pn(2, 2);
+      Matrix2 Dpi_pn;
       const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
 
       // chain the Jacobian matrices
@@ -392,7 +392,7 @@ public:
       const Point3& pw, //
       boost::optional<Matrix&> Dpose = boost::none,
       boost::optional<Matrix&> Dpoint = boost::none,
-      boost::optional<Matrix&> Dcal = boost::none) const {
+      boost::optional<Matrix25&> Dcal = boost::none) const {
 
     if (!Dpose && !Dpoint && !Dcal) {
       const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter)
@@ -412,7 +412,7 @@ public:
     const Point2 pn = project_to_camera(pc, Dpn_pc);
 
     // uncalibration
-    Matrix Dpi_pn; // 2*2
+    Matrix2 Dpi_pn; // 2*2
     const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
 
     // chain the Jacobian matrices
diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp
index 86c0b7e33..aa06a2e29 100644
--- a/gtsam/geometry/tests/testCal3_S2.cpp
+++ b/gtsam/geometry/tests/testCal3_S2.cpp
@@ -60,7 +60,7 @@ TEST( Cal3_S2, calibrate_homogeneous) {
 Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) { return k.uncalibrate(pt); }
 TEST( Cal3_S2, Duncalibrate1)
 {
-  Matrix computed; K.uncalibrate(p, computed, boost::none);
+  Matrix25 computed; K.uncalibrate(p, computed, boost::none);
   Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
   CHECK(assert_equal(numerical,computed,1e-8));
 }
diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h
index d9e7b9819..5d57d2862 100644
--- a/gtsam/slam/SmartFactorBase.h
+++ b/gtsam/slam/SmartFactorBase.h
@@ -280,7 +280,7 @@ public:
 
       Vector bi;
       try {
-        bi = -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector();
+        bi = -(cameras[i].project(point, Fi, Ei, static_cast<Matrix25&>(Hcali)) - this->measured_.at(i)).vector();
         if(body_P_sensor_){
           Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse());
           Matrix J(6, 6);