Speeding up localCoordinates

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();
 }
 

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 {

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;
   }