From 8619b04cd7b01ebab658aef10a231d78db62e0a6 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 22 Feb 2015 22:52:31 +0100
Subject: [PATCH] Now switched to full ZDim*Dim blocks, no more hacky
 calibration splitting...

---
 gtsam/geometry/CameraSet.h             | 26 +++++++++-----------------
 gtsam/geometry/tests/testCameraSet.cpp | 15 ++++++---------
 2 files changed, 15 insertions(+), 26 deletions(-)

diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h
index 43b806857..d0c2d04d0 100644
--- a/gtsam/geometry/CameraSet.h
+++ b/gtsam/geometry/CameraSet.h
@@ -27,7 +27,6 @@ namespace gtsam {
 
 /**
  * @brief A set of cameras, all with their own calibration
- * Assumes that a camera is laid out as 6 Pose3 parameters then calibration
  */
 template<class CAMERA>
 class CameraSet: public std::vector<CAMERA> {
@@ -64,8 +63,8 @@ protected:
 public:
 
   /// Definitions for blocks of F
-  typedef Eigen::Matrix<double, ZDim, 6> MatrixZ6; // F
-  typedef std::vector<MatrixZ6> FBlocks;
+  typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F
+  typedef std::vector<MatrixZD> FBlocks;
 
   /**
    * print
@@ -92,15 +91,14 @@ public:
   }
 
   /**
-   * Project a point, with derivatives in this, point, and calibration
+   * Project a point, with derivatives in CameraSet and Point3
    * Note that F is a sparse block-diagonal matrix, so instead of a large dense
    * matrix this function returns the diagonal blocks.
    * throws CheiralityException
    */
-  std::vector<Z> project(const Point3& point, //
+  std::vector<Z> project2(const Point3& point, //
       boost::optional<FBlocks&> F = boost::none, //
-      boost::optional<Matrix&> E = boost::none, //
-      boost::optional<Matrix&> H = boost::none) const {
+      boost::optional<Matrix&> E = boost::none) const {
 
     // Allocate result
     size_t m = this->size();
@@ -109,22 +107,16 @@ public:
     // Allocate derivatives
     if (E)
       E->resize(ZDim * m, 3);
-    if (H && Dim > 6)
-      H->resize(ZDim * m, Dim - 6);
-
-    Eigen::Matrix<double, ZDim, 6> Fi;
-    Eigen::Matrix<double, ZDim, 3> Ei;
-    Eigen::Matrix<double, ZDim, Dim - 6> Hi;
 
     // Project and fill derivatives
     for (size_t i = 0; i < m; i++) {
-      z[i] = this->at(i).project(point, F ? &Fi : 0, E ? &Ei : 0, H ? &Hi : 0);
+      Eigen::Matrix<double, ZDim, Dim> Fi;
+      Eigen::Matrix<double, ZDim, 3> Ei;
+      z[i] = this->at(i).project2(point, F ? &Fi : 0, E ? &Ei : 0);
       if (F)
         F->push_back(Fi);
       if (E)
         E->block<ZDim, 3>(ZDim * i, 0) = Ei;
-      if (H)
-        H->block<ZDim, Dim - 6>(ZDim * i, 0) = Hi;
     }
 
     return z;
@@ -150,7 +142,7 @@ public:
   /// Calculate vector of re-projection errors
   Vector reprojectionErrors(const Point3& point,
       const std::vector<Z>& measured) const {
-    return ErrorVector(project(point), measured);
+    return ErrorVector(project2(point), measured);
   }
 
   /// Calculate vector of re-projection errors, from point at infinity
diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp
index b4f8a75ef..a003b6bce 100644
--- a/gtsam/geometry/tests/testCameraSet.cpp
+++ b/gtsam/geometry/tests/testCameraSet.cpp
@@ -44,31 +44,28 @@ TEST(CameraSet, Pinhole) {
 
   // Check measurements
   Point2 expected;
-  ZZ z = set.project(p);
+  ZZ z = set.project2(p);
   EXPECT(assert_equal(expected, z[0]));
   EXPECT(assert_equal(expected, z[1]));
 
   // Calculate expected derivatives using Pinhole
   Matrix43 actualE;
-  Matrix43 actualH;
   Matrix F1;
   {
     Matrix23 E1;
     Matrix23 H1;
-    camera.project(p, F1, E1, H1);
+    camera.project2(p, F1, E1);
     actualE << E1, E1;
-    actualH << H1, H1;
   }
 
   // Check computed derivatives
   CameraSet<Camera>::FBlocks F;
   Matrix E, H;
-  set.project(p, F, E, H);
+  set.project2(p, F, E);
   LONGS_EQUAL(2,F.size());
   EXPECT(assert_equal(F1, F[0]));
   EXPECT(assert_equal(F1, F[1]));
   EXPECT(assert_equal(actualE, E));
-  EXPECT(assert_equal(actualH, H));
 
   // Check errors
   ZZ measured;
@@ -102,7 +99,7 @@ TEST(CameraSet, Stereo) {
 
   // Check measurements
   StereoPoint2 expected(0, -1, 0);
-  ZZ z = set.project(p);
+  ZZ z = set.project2(p);
   EXPECT(assert_equal(expected, z[0]));
   EXPECT(assert_equal(expected, z[1]));
 
@@ -111,14 +108,14 @@ TEST(CameraSet, Stereo) {
   Matrix F1;
   {
     Matrix33 E1;
-    camera.project(p, F1, E1);
+    camera.project2(p, F1, E1);
     actualE << E1, E1;
   }
 
   // Check computed derivatives
   CameraSet<StereoCamera>::FBlocks F;
   Matrix E;
-  set.project(p, F, E);
+  set.project2(p, F, E);
   LONGS_EQUAL(2,F.size());
   EXPECT(assert_equal(F1, F[0]));
   EXPECT(assert_equal(F1, F[1]));