From a7f9c861ad8a549346e8998171b9907a4775944a Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 23 Mar 2012 03:39:05 +0000
Subject: [PATCH] Added range functions in cameras (in branch, not tested yet)

---
 gtsam/geometry/CalibratedCamera.h | 32 +++++++++++-
 gtsam/geometry/PinholeCamera.h    | 83 ++++++++++++++++++++++++++++---
 2 files changed, 107 insertions(+), 8 deletions(-)

diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h
index 95ff66e68..04296eba1 100644
--- a/gtsam/geometry/CalibratedCamera.h
+++ b/gtsam/geometry/CalibratedCamera.h
@@ -120,7 +120,7 @@ namespace gtsam {
 		/* ************************************************************************* */
 
     /// @}
-    /// @name Transformations
+    /// @name Transformations and mesaurement functions
     /// @{
 
 		/**
@@ -147,6 +147,36 @@ namespace gtsam {
 		 */
 		static Point3 backproject_from_camera(const Point2& p, const double scale);
 
+    /**
+     * Calculate range to a landmark
+     * @param point 3D location of landmark
+     * @return range (double)
+     */
+    double range(const Point3& point,
+        boost::optional<Matrix&> H1=boost::none,
+        boost::optional<Matrix&> H2=boost::none) const {
+      return pose_.range(point, H1, H2); }
+
+    /**
+     * Calculate range to another pose
+     * @param pose Other SO(3) pose
+     * @return range (double)
+     */
+    double range(const Pose3& pose,
+        boost::optional<Matrix&> H1=boost::none,
+        boost::optional<Matrix&> H2=boost::none) const {
+      return pose_.range(pose, H1, H2); }
+
+    /**
+     * Calculate range to another camera
+     * @param camera Other camera
+     * @return range (double)
+     */
+    double range(const CalibratedCamera& camera,
+        boost::optional<Matrix&> H1=boost::none,
+        boost::optional<Matrix&> H2=boost::none) const {
+      return pose_.range(camera.pose_, H1, H2); }
+
 private:
 
     /// @}
diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h
index 8aee12064..1d9e9dcff 100644
--- a/gtsam/geometry/PinholeCamera.h
+++ b/gtsam/geometry/PinholeCamera.h
@@ -161,9 +161,9 @@ namespace gtsam {
         return PinholeCamera(pose3, K);
     }
 
-    /* ************************************************************************* */
-    // measurement functions and derivatives
-    /* ************************************************************************* */
+    /// @}
+    /// @name Transformations and measurement functions
+    /// @{
 
     /**
     * projects a 3-dimensional point in camera coordinates into the
@@ -186,10 +186,6 @@ namespace gtsam {
       return std::make_pair(k_.uncalibrate(pn), pc.z()>0);
     }
 
-    /// @}
-    /// @name Transformations
-    /// @{
-
     /** project a point from world coordinate to the image
      *  @param pw is a point in the world coordinate
      *  @param H1 is the jacobian w.r.t. pose3
@@ -270,6 +266,79 @@ namespace gtsam {
       return backproject(pi, scale);
     }
 
+    /**
+     * Calculate range to a landmark
+     * @param point 3D location of landmark
+     * @return range (double)
+     */
+    double range(const Point3& point,
+        boost::optional<Matrix&> H1=boost::none,
+        boost::optional<Matrix&> H2=boost::none) const {
+      double result = pose_.range(point, H1, H2);
+      if(H1) {
+        // Add columns of zeros to Jacobian for calibration
+        Matrix& H1r(*H1);
+        H1r.conservativeResize(Eigen::NoChange, pose_.dim() + k_.dim());
+        H1r.block(0, pose_.dim(), 1, k_.dim()) = Matrix::Zero(1, k_.dim());
+        gtsam::print(*H1, "H1: ");
+      }
+      std::cout << "Range = " << result << std::endl;
+      return result;
+    }
+
+    /**
+     * Calculate range to another pose
+     * @param pose Other SO(3) pose
+     * @return range (double)
+     */
+    double range(const Pose3& pose,
+        boost::optional<Matrix&> H1=boost::none,
+        boost::optional<Matrix&> H2=boost::none) const {
+      double result = pose_.range(pose, H1, H2);
+      if(H1) {
+        // Add columns of zeros to Jacobian for calibration
+        Matrix& H1r(*H1);
+        H1r.conservativeResize(Eigen::NoChange, pose_.dim() + k_.dim());
+        H1r.block(0, pose_.dim(), 1, k_.dim()) = Matrix::Zero(1, k_.dim());
+      }
+      return result;
+    }
+
+    /**
+     * Calculate range to another camera
+     * @param camera Other camera
+     * @return range (double)
+     */
+    template<class CalibrationB>
+    double range(const PinholeCamera<CalibrationB>& camera,
+        boost::optional<Matrix&> H1=boost::none,
+        boost::optional<Matrix&> H2=boost::none) const {
+      double result = pose_.range(camera.pose_, H1, H2);
+      if(H1) {
+        // Add columns of zeros to Jacobian for calibration
+        Matrix& H1r(*H1);
+        H1r.conservativeResize(Eigen::NoChange, pose_.dim() + k_.dim());
+        H1r.block(0, pose_.dim(), 1, k_.dim()) = Matrix::Zero(1, k_.dim());
+      }
+      if(H2) {
+        // Add columns of zeros to Jacobian for calibration
+        Matrix& H2r(*H2);
+        H2r.conservativeResize(Eigen::NoChange, camera.pose().dim() + camera.calibration().dim());
+        H2r.block(0, camera.pose().dim(), 1, camera.calibration().dim()) = Matrix::Zero(1, camera.calibration().dim());
+      }
+      return result;
+    }
+
+    /**
+     * Calculate range to another camera
+     * @param camera Other camera
+     * @return range (double)
+     */
+    double range(const CalibratedCamera& camera,
+        boost::optional<Matrix&> H1=boost::none,
+        boost::optional<Matrix&> H2=boost::none) const {
+      return pose_.range(camera.pose_, H1, H2); }
+
 private:
 
     /// @}