From e46be6021539e0f066729ee5dfd8115a5e8c10dd Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Wed, 22 Oct 2014 13:46:05 +0200
Subject: [PATCH] Speeding up localCoordinates

---
 gtsam/geometry/Cal3Bundler.cpp |  4 ++--
 gtsam/geometry/Cal3Bundler.h   |  4 ++--
 gtsam/geometry/PinholeCamera.h | 11 ++++++-----
 3 files changed, 10 insertions(+), 9 deletions(-)

diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp
index 95b61c2b0..ab5fde629 100644
--- a/gtsam/geometry/Cal3Bundler.cpp
+++ b/gtsam/geometry/Cal3Bundler.cpp
@@ -46,7 +46,7 @@ Vector Cal3Bundler::k() const {
 }
 
 /* ************************************************************************* */
-Vector Cal3Bundler::vector() const {
+Vector3 Cal3Bundler::vector() const {
   return (Vector(3) << f_, k1_, k2_);
 }
 
@@ -189,7 +189,7 @@ Cal3Bundler Cal3Bundler::retract(const Vector& d) const {
 }
 
 /* ************************************************************************* */
-Vector Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const {
+Vector3 Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const {
   return T2.vector() - vector();
 }
 
diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index 454435c66..793f195d5 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -70,7 +70,7 @@ public:
   Matrix K() const; ///< Standard 3*3 calibration matrix
   Vector k() const; ///< Radial distortion parameters (4 of them, 2 0)
 
-  Vector vector() const;
+  Vector3 vector() const;
 
   /// focal length x
   inline double fx() const {
@@ -150,7 +150,7 @@ public:
   Cal3Bundler retract(const Vector& d) const;
 
   /// Calculate local coordinates to another calibration
-  Vector localCoordinates(const Cal3Bundler& T2) const;
+  Vector3 localCoordinates(const Cal3Bundler& T2) const;
 
   /// dimensionality
   virtual size_t dim() const {
diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h
index 6df1af24f..2514e4a92 100644
--- a/gtsam/geometry/PinholeCamera.h
+++ b/gtsam/geometry/PinholeCamera.h
@@ -242,12 +242,13 @@ public:
           calibration().retract(d.tail(calibration().dim())));
   }
 
+  typedef Eigen::Matrix<double,6+DimK,1> VectorK6;
+
   /// return canonical coordinate
-  Vector localCoordinates(const PinholeCamera& T2) const {
-    Vector d(dim());
-    d.head(pose().dim()) = pose().localCoordinates(T2.pose());
-    d.tail(calibration().dim()) = calibration().localCoordinates(
-        T2.calibration());
+  VectorK6 localCoordinates(const PinholeCamera& T2) const {
+    VectorK6 d; // TODO: why does d.head<6>() not compile??
+    d.head(6) = pose().localCoordinates(T2.pose());
+    d.tail(DimK) = calibration().localCoordinates(T2.calibration());
     return d;
   }