Fixed size vectors for all Logmap, localcoordinates and vector methods

gtsam/base/Vector.h

@@ -34,6 +34,7 @@ namespace gtsam {
 typedef Eigen::VectorXd Vector;
 
 // Commonly used fixed size vectors
+typedef Eigen::Matrix<double, 1, 1> Vector1;
 typedef Eigen::Vector2d Vector2;
 typedef Eigen::Vector3d Vector3;
 typedef Eigen::Matrix<double, 4, 1> Vector4;
@@ -42,6 +43,7 @@ typedef Eigen::Matrix<double, 6, 1> Vector6;
 typedef Eigen::Matrix<double, 7, 1> Vector7;
 typedef Eigen::Matrix<double, 8, 1> Vector8;
 typedef Eigen::Matrix<double, 9, 1> Vector9;
+typedef Eigen::Matrix<double, 10, 1> Vector10;
 
 typedef Eigen::VectorBlock<Vector> SubVector;
 typedef Eigen::VectorBlock<const Vector> ConstSubVector;

gtsam/geometry/Cal3Bundler.cpp

@@ -49,7 +49,7 @@ Vector4 Cal3Bundler::k() const {
 
 /* ************************************************************************* */
 Vector3 Cal3Bundler::vector() const {
-  return (Vector(3) << f_, k1_, k2_).finished();
+  return Vector3(f_, k1_, k2_);
 }
 
 /* ************************************************************************* */

gtsam/geometry/Cal3DS2_Base.cpp

@@ -35,8 +35,10 @@ Matrix3 Cal3DS2_Base::K() const {
 }
 
 /* ************************************************************************* */
-Vector Cal3DS2_Base::vector() const {
-  return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_).finished();
+Vector9 Cal3DS2_Base::vector() const {
+  Vector9 v;
+  v << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_;
+  return v;
 }
 
 /* ************************************************************************* */

gtsam/geometry/Cal3DS2_Base.h

@@ -46,8 +46,8 @@ protected:
 
 public:
   Matrix3 K() const ;
-  Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); }
-  Vector vector() const ;
+  Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
+  Vector9 vector() const ;
 
   /// @name Standard Constructors
   /// @{

gtsam/geometry/Cal3Unified.cpp

@@ -29,8 +29,10 @@ Cal3Unified::Cal3Unified(const Vector &v):
     Base(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {}
 
 /* ************************************************************************* */
-Vector Cal3Unified::vector() const {
-  return (Vector(10) << Base::vector(), xi_).finished();
+Vector10 Cal3Unified::vector() const {
+  Vector10 v;
+  v << Base::vector(), xi_;
+  return v;
 }
 
 /* ************************************************************************* */
@@ -133,7 +135,7 @@ Cal3Unified Cal3Unified::retract(const Vector& d) const {
 }
 
 /* ************************************************************************* */
-Vector Cal3Unified::localCoordinates(const Cal3Unified& T2) const {
+Vector10 Cal3Unified::localCoordinates(const Cal3Unified& T2) const {
   return T2.vector() - vector();
 }
 

gtsam/geometry/Cal3Unified.h

@@ -51,7 +51,7 @@ private:
 
 public:
 
-    Vector vector() const ;
+    Vector10 vector() const ;
 
   /// @name Standard Constructors
   /// @{
@@ -116,7 +116,7 @@ public:
   Cal3Unified retract(const Vector& d) const ;
 
   /// Given a different calibration, calculate update to obtain it
-  Vector localCoordinates(const Cal3Unified& T2) const ;
+  Vector10 localCoordinates(const Cal3Unified& T2) const ;
 
   /// Return dimensions of calibration manifold object
   virtual size_t dim() const { return 10 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)

gtsam/geometry/EssentialMatrix.cpp

@@ -51,9 +51,11 @@ EssentialMatrix EssentialMatrix::retract(const Vector& xi) const {
 }
 
 /* ************************************************************************* */
-Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const {
-  return (Vector(5) << aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(
-      other.aTb_)).finished();
+Vector5 EssentialMatrix::localCoordinates(const EssentialMatrix& other) const {
+  Vector5 v;
+  v << aRb_.localCoordinates(other.aRb_),
+      aTb_.localCoordinates(other.aTb_);
+  return v;
 }
 
 /* ************************************************************************* */

gtsam/geometry/EssentialMatrix.h

@@ -31,8 +31,8 @@ private:
 public:
 
   /// Static function to convert Point2 to homogeneous coordinates
-  static Vector Homogeneous(const Point2& p) {
-    return (Vector(3) << p.x(), p.y(), 1).finished();
+  static Vector3 Homogeneous(const Point2& p) {
+    return Vector3(p.x(), p.y(), 1);
   }
 
   /// @name Constructors and named constructors
@@ -84,15 +84,15 @@ public:
   }
 
   /// Return the dimensionality of the tangent space
-  virtual size_t dim() const {
+  size_t dim() const {
     return 5;
   }
 
   /// Retract delta to manifold
-  virtual EssentialMatrix retract(const Vector& xi) const;
+  EssentialMatrix retract(const Vector& xi) const;
 
   /// Compute the coordinates in the tangent space
-  virtual Vector localCoordinates(const EssentialMatrix& other) const;
+  Vector5 localCoordinates(const EssentialMatrix& other) const;
 
   /// @}
 

gtsam/geometry/Point2.h

@@ -171,7 +171,7 @@ public:
   static inline Point2 Expmap(const Vector& v) { return Point2(v); }
 
   /// Log map around identity - just return the Point2 as a vector
-  static inline Vector Logmap(const Point2& dp) { return (Vector(2) << dp.x(), dp.y()).finished(); }
+  static inline Vector2 Logmap(const Point2& dp) { return Vector2(dp.x(), dp.y()); }
 
   /// @}
   /// @name Vector Space

gtsam/geometry/Pose2.cpp

@@ -75,18 +75,18 @@ Pose2 Pose2::Expmap(const Vector& xi) {
 }
 
 /* ************************************************************************* */
-Vector Pose2::Logmap(const Pose2& p) {
+Vector3 Pose2::Logmap(const Pose2& p) {
   const Rot2& R = p.r();
   const Point2& t = p.t();
   double w = R.theta();
   if (std::abs(w) < 1e-10)
-    return (Vector(3) << t.x(), t.y(), w).finished();
+    return Vector3(t.x(), t.y(), w);
   else {
     double c_1 = R.c()-1.0, s = R.s();
     double det = c_1*c_1 + s*s;
     Point2 p = R_PI_2 * (R.unrotate(t) - t);
     Point2 v = (w/det) * p;
-    return (Vector(3) << v.x(), v.y(), w).finished();
+    return Vector3(v.x(), v.y(), w);
   }
 }
 
@@ -101,12 +101,12 @@ Pose2 Pose2::retract(const Vector& v) const {
 }
 
 /* ************************************************************************* */
-Vector Pose2::localCoordinates(const Pose2& p2) const {
+Vector3 Pose2::localCoordinates(const Pose2& p2) const {
 #ifdef SLOW_BUT_CORRECT_EXPMAP
   return Logmap(between(p2));
 #else
   Pose2 r = between(p2);
-  return (Vector(3) << r.x(), r.y(), r.theta()).finished();
+  return Vector3(r.x(), r.y(), r.theta());
 #endif
 }
 

gtsam/geometry/Pose2.h

@@ -139,7 +139,7 @@ public:
   Pose2 retract(const Vector& v) const;
 
   /// Local 3D coordinates \f$ [T_x,T_y,\theta] \f$ of Pose2 manifold neighborhood around current pose
-  Vector localCoordinates(const Pose2& p2) const;
+  Vector3 localCoordinates(const Pose2& p2) const;
 
   /// @}
   /// @name Lie Group
@@ -149,7 +149,7 @@ public:
   static Pose2 Expmap(const Vector& xi);
 
   ///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation
-  static Vector Logmap(const Pose2& p);
+  static Vector3 Logmap(const Pose2& p);
 
   /**
    * Calculate Adjoint map
@@ -169,10 +169,11 @@ public:
    * @return xihat, 3*3 element of Lie algebra that can be exponentiated
    */
   static inline Matrix3 wedge(double vx, double vy, double w) {
-     return (Matrix(3,3) <<
-        0.,-w,  vx,
-        w,  0., vy,
-        0., 0.,  0.).finished();
+    Matrix3 m;
+    m << 0.,-w,  vx,
+         w,  0., vy,
+         0., 0.,  0.;
+    return m;
   }
 
   /// @}

gtsam/geometry/Rot2.h

@@ -154,7 +154,7 @@ namespace gtsam {
     inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); }
 
     /// Returns inverse retraction
-    inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); }
+    inline Vector1 localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); }
 
     /// @}
     /// @name Lie Group
@@ -169,8 +169,10 @@ namespace gtsam {
     }
 
     ///Log map at identity - return the canonical coordinates of this rotation
-    static inline Vector Logmap(const Rot2& r) {
-      return (Vector(1) << r.theta()).finished();
+    static inline Vector1 Logmap(const Rot2& r) {
+      Vector1 v;
+      v << r.theta();
+      return v;
     }
 
     /// @}

gtsam/geometry/Rot3.cpp

@@ -52,7 +52,7 @@ Rot3 Rot3::Random(boost::mt19937 & rng) {
 }
 
 /* ************************************************************************* */
-Rot3 Rot3::rodriguez(const Vector& w) {
+Rot3 Rot3::rodriguez(const Vector3& w) {
   double t = w.norm();
   if (t < 1e-10) return Rot3();
   return rodriguez(w/t, t);

gtsam/geometry/Rot3.h

@@ -183,7 +183,7 @@ namespace gtsam {
      * @param v a vector of incremental roll,pitch,yaw
      * @return incremental rotation matrix
      */
-    static Rot3 rodriguez(const Vector& v);
+    static Rot3 rodriguez(const Vector3& v);
 
     /**
      * Rodriguez' formula to compute an incremental rotation matrix
@@ -193,7 +193,7 @@ namespace gtsam {
      * @return incremental rotation matrix
      */
     static Rot3 rodriguez(double wx, double wy, double wz)
-      { return rodriguez((Vector(3) << wx, wy, wz).finished());}
+      { return rodriguez(Vector3(wx, wy, wz));}
 
     /// @}
     /// @name Testable

gtsam/geometry/StereoCamera.cpp

@@ -91,11 +91,11 @@ namespace gtsam {
     double d = 1.0 / P.z(), d2 = d*d;
     const Cal3_S2Stereo& K = *K_;
     double f_x = K.fx(), f_y = K.fy(), b = K.baseline();
-    return (Matrix(3, 3) <<
-         f_x*d,   0.0, -d2*f_x* P.x(),
+    Matrix3 m;
+    m << f_x*d,   0.0, -d2*f_x* P.x(),
          f_x*d,   0.0, -d2*f_x*(P.x() - b),
-           0.0, f_y*d, -d2*f_y* P.y()
-    ).finished();
+           0.0, f_y*d, -d2*f_y* P.y();
+    return m;
   }
 
 }

gtsam/geometry/StereoCamera.h

@@ -90,14 +90,8 @@ public:
   }
 
   /// Local coordinates of manifold neighborhood around current value
-  inline Vector localCoordinates(const StereoCamera& t2) const {
-    return Vector(leftCamPose_.localCoordinates(t2.leftCamPose_));
-  }
-
-  Pose3 between(const StereoCamera &camera,
-      OptionalJacobian<6,6> H1=boost::none,
-      OptionalJacobian<6,6> H2=boost::none) const {
-    return leftCamPose_.between(camera.pose(), H1, H2);
+  inline Vector6 localCoordinates(const StereoCamera& t2) const {
+    return leftCamPose_.localCoordinates(t2.leftCamPose_);
   }
 
   /// @}

gtsam/geometry/Unit3.cpp

@@ -164,7 +164,7 @@ Vector2 Unit3::localCoordinates(const Unit3& y) const {
   if (std::abs(dot - 1.0) < 1e-16)
     return Vector2(0, 0);
   else if (std::abs(dot + 1.0) < 1e-16)
-    return (Vector(2) << M_PI, 0).finished();
+    return Vector2(M_PI, 0);
   else {
     // no special case
     double theta = acos(dot);