From e48b38ca21da7fd5ad75f1f8638705365abf9f54 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Mon, 6 Oct 2014 15:45:26 +0200
Subject: [PATCH] Fixing uncalibrate (does not yet compile)

---
 gtsam/base/Matrix.h                       | 2 ++
 gtsam/geometry/Cal3_S2.cpp                | 9 +++++++++
 gtsam/geometry/Cal3_S2.h                  | 9 +++++++--
 gtsam_unstable/nonlinear/Expression-inl.h | 2 ++
 4 files changed, 20 insertions(+), 2 deletions(-)

diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h
index 689f36baa..44ff1703f 100644
--- a/gtsam/base/Matrix.h
+++ b/gtsam/base/Matrix.h
@@ -37,8 +37,10 @@ namespace gtsam {
 typedef Eigen::MatrixXd Matrix;
 typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor;
 
+typedef Eigen::Matrix2d Matrix2;
 typedef Eigen::Matrix3d Matrix3;
 typedef Eigen::Matrix4d Matrix4;
+typedef Eigen::Matrix<double,5,5> Matrix5;
 typedef Eigen::Matrix<double,6,6> Matrix6;
 
 typedef Eigen::Matrix<double,2,3> Matrix23;
diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp
index c82b36a83..133f1821c 100644
--- a/gtsam/geometry/Cal3_S2.cpp
+++ b/gtsam/geometry/Cal3_S2.cpp
@@ -86,6 +86,15 @@ Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
   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 {
+  const double x = p.x(), y = p.y();
+  if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
+  if (Dp) *Dp << fx_, s_, 0.0, fy_;
+  return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
+}
+
 /* ************************************************************************* */
 Point2 Cal3_S2::calibrate(const Point2& p) const {
   const double u = p.x(), v = p.y();
diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h
index 03c6bff3f..c7996f5d2 100644
--- a/gtsam/geometry/Cal3_S2.h
+++ b/gtsam/geometry/Cal3_S2.h
@@ -36,6 +36,8 @@ private:
   double fx_, fy_, s_, u0_, v0_;
 
 public:
+  /// dimension of the variable - used to autodetect sizes
+  static const size_t dimension = 5;
 
   typedef boost::shared_ptr<Cal3_S2> shared_ptr; ///< shared pointer to calibration object
 
@@ -151,6 +153,9 @@ public:
   Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal =
       boost::none, boost::optional<Matrix&> Dp = boost::none) const;
 
+  Point2 uncalibrate(const Point2& p, boost::optional<Matrix25&> Dcal =
+      boost::none, boost::optional<Matrix2&> Dp = boost::none) const;
+
   /**
    * convert image coordinates uv to intrinsic coordinates xy
    * @param p point in image coordinates
@@ -181,12 +186,12 @@ public:
 
   /// return DOF, dimensionality of tangent space
   inline size_t dim() const {
-    return 5;
+    return dimension;
   }
 
   /// return DOF, dimensionality of tangent space
   static size_t Dim() {
-    return 5;
+    return dimension;
   }
 
   /// Given 5-dim tangent vector, create new calibration
diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h
index ab58dbf4c..fb0af0d54 100644
--- a/gtsam_unstable/nonlinear/Expression-inl.h
+++ b/gtsam_unstable/nonlinear/Expression-inl.h
@@ -383,6 +383,8 @@ class BinaryExpression: public ExpressionNode<T> {
 
 public:
 
+  typedef Eigen::Matrix<double,T::dimension,A1::dimension> JacobianTA1;
+  typedef Eigen::Matrix<double,T::dimension,A2::dimension> JacobianTA2;
   typedef boost::function<
       T(const A1&, const A2&, boost::optional<Matrix&>,
           boost::optional<Matrix&>)> Function;