Improvements to the Lie objects, with new member/static functions in …

…most of the geometry objects. Many of the functions that were previously global have been moved to static functions. See Lie.h for more details.

base/Lie-inl.h

@@ -18,10 +18,10 @@
   template class Lie<T>;
 
 namespace gtsam {
-  template<class T>
-  size_t Lie<T>::dim() const {
-    return gtsam::dim(*((T*)this));
-  }
+//  template<class T>
+//  size_t Lie<T>::dim() const {
+//    return gtsam::dim(*((T*)this));
+//  }
 
   /**
    * Returns Exponential mapy

base/Lie.h

@@ -1,37 +1,55 @@
-/*
- * Lie.h
- *
- *  Created on: Jan 5, 2010
- *      Author: Richard Roberts
+/**
+ * @file Lie.h
+ * @brief Base class and basic functions for Lie types
+ * @author Richard Roberts
+ * @author Alex Cunningham
  */
 
 #pragma once
 
 #include <string>
+
 #include <gtsam/base/Matrix.h>
 
 namespace gtsam {
 
+  /**
+   * These core global functions can be specialized by new Lie types
+   * for better performance.
+   */
+
+  /* Exponential map about identity */
   template<class T>
-  T expmap(const Vector& v);  /* Exponential map about identity */
+  T expmap(const Vector& v) { return T::Expmap(v); }
 
-  // The following functions may be overridden in your own class file
-  // with more efficient versions if possible.
+  /* Logmap (inverse exponential map) about identity */
+  template<class T>
+  Vector logmap(const T& p) { return T::Logmap(p); }
 
-  // Compute l1 s.t. l2=l1*l0
+  /** Compute l1 s.t. l2=l1*l0 */
   template<class T>
   inline T between(const T& l1, const T& l2) { return compose(inverse(l1),l2); }
 
-  // Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp
+  /** Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp */
   template<class T>
   inline Vector logmap(const T& l0, const T& lp) { return logmap(between(l0,lp)); }
 
-  /* Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */
+  /** Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */
   template<class T>
   inline T expmap(const T& t, const Vector& d) { return compose(t,expmap<T>(d)); }
 
   /**
    * Base class for Lie group type
+   * This class uses the Curiously Recurring Template design pattern to allow
+   * for static polymorphism.
+   *
+   * T is the derived Lie type, like Point2, Pose3, etc.
+   *
+   * By convention, we use capital letters to designate a static function
+   *
+   * FIXME: Need to find a way to check for actual implementations in T
+   * so that there are no recursive function calls.  This could be handled
+   * by not using the same name
    */
   template <class T>
   class Lie {
@@ -40,23 +58,65 @@ namespace gtsam {
     /**
      * Returns dimensionality of the tangent space
      */
-    size_t dim() const;
+    inline size_t dim() const {
+    	return static_cast<const T*>(this)->dim();
+    }
 
     /**
-     * Returns Exponential mapy
+     * Returns Exponential map update of T
+     * Default implementation calls global binary function
      */
     T expmap(const Vector& v) const;
 
+    /** expmap around identity */
+    static T Expmap(const Vector& v) {
+    	return T::Expmap(v);
+    }
+
     /**
      * Returns Log map
+     * Default Implementation calls global binary function
      */
     Vector logmap(const T& lp) const;
 
+    /** Logmap around identity */
+    static Vector Logmap(const T& p) {
+    	return T::Logmap(p);
+    }
+
+    /** compose with another object */
+    inline T compose(const T& p) const {
+    	return static_cast<const T*>(this)->compose(p);
+    }
+
+    /** invert the object and yield a new one */
+    inline T inverse() const {
+    	return static_cast<const T*>(this)->inverse();
+    }
+
   };
 
+  /** get the dimension of an object with a global function */
+  template<class T>
+  inline size_t dim(const T& object) {
+	  return object.dim();
+  }
+
+  /** compose two Lie types */
+  template<class T>
+  inline T compose(const T& p1, const T& p2) {
+	  return p1.compose(p2);
+  }
+
+  /** invert an object */
+  template<class T>
+  inline T inverse(const T& p) {
+	  return p.inverse();
+  }
+
   /** Call print on the object */
   template<class T>
-  inline void print_(const T& object, const std::string& s = "") {
+  inline void print(const T& object, const std::string& s = "") {
     object.print(s);
   }
 
@@ -100,11 +160,11 @@ namespace gtsam {
   inline Vector logmap(const Vector& p1,const Vector& p2) { return p2-p1;}
 
   /**
-   *  Three term approximation of the BakerÐCampbellÐHausdorff formula
+   *  Three term approximation of the Baker�Campbell�Hausdorff formula
    *  In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y)
    *  it is not true that Z = X+Y. Instead, Z can be calculated using the BCH
    *  formula: Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24
-   *  http://en.wikipedia.org/wiki/BakerÐCampbellÐHausdorff_formula
+   *  http://en.wikipedia.org/wiki/Baker�Campbell�Hausdorff_formula
    */
   template<class T>
   T BCH(const T& X, const T& Y) {

geometry/Point2.h

@@ -22,8 +22,7 @@ namespace gtsam {
   class Point2: Testable<Point2>, public Lie<Point2> {
   public:
 	  /// dimension of the variable - used to autodetect sizes
-	  static inline size_t dim() {return 2;}
-
+	  static const size_t dimension = 2;
   private:
     double x_, y_;
 
@@ -33,12 +32,32 @@ namespace gtsam {
     Point2(double x, double y): x_(x), y_(y) {}
     Point2(const Vector& v) : x_(v(0)), y_(v(1)) {}
 
+    /** dimension of the variable - used to autodetect sizes */
+    inline static size_t Dim() { return dimension; }
+
     /** print with optional string */
     void print(const std::string& s = "") const;
 
     /** equals with an tolerance, prints out message if unequal*/
     bool equals(const Point2& q, double tol = 1e-9) const;
 
+    /** Lie requirements */
+
+    /** Size of the tangent space of the Lie type */
+    inline size_t dim() const { return dimension; }
+
+    /** "Compose", just adds the coordinates of two points. */
+    Point2 compose(const Point2& p1) const { return *this+p1; }
+
+    /** "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() */
+    Point2 inverse() const { return Point2(-x_, -y_); }
+
+    /** Exponential map around identity - just create a Point2 from a vector */
+    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()); }
+
     /** get functions for x, y */
     double x() const {return x_;}
     double y() const {return y_;}
@@ -75,20 +94,7 @@ namespace gtsam {
 
   /** Lie group functions */
 
-  /** Global print calls member function */
-  inline void print(const Point2& p, const std::string& s = "Point2") { p.print(s); }
-
-  /** Dimensionality of the tangent space */
-  inline size_t dim(const Point2& obj) { return 2; }
-
-  /** Exponential map around identity - just create a Point2 from a vector */
-  template<> inline Point2 expmap(const Vector& dp) { return Point2(dp); }
-
-  /** Log map around identity - just return the Point2 as a vector */
-  inline Vector logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); }
-
   /** "Compose", just adds the coordinates of two points. */
-  inline Point2 compose(const Point2& p1, const Point2& p2) { return p1+p2; }
   inline Point2 compose(const Point2& p1, const Point2& p2,
       boost::optional<Matrix&> H1, boost::optional<Matrix&> H2=boost::none) {
     if(H1) *H1 = eye(2);
@@ -113,9 +119,6 @@ namespace gtsam {
     return between(p1, p2);
   }
 
-  /** "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() */
-  inline Point2 inverse(const Point2& p) { return Point2(-p.x(), -p.y()); }
-
   /** multiply with scalar */
   inline Point2 operator*(double s, const Point2& p) {return p*s;}
 }

geometry/Point3.cpp

@@ -17,6 +17,8 @@ namespace gtsam {
     return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol);
   }
 
+  /* ************************************************************************* */
+
   void Point3::print(const std::string& s) const {
     std::cout << s << "(" << x_ << ", " << y_ <<  ", " << z_ << ")" << std::endl;
   }

geometry/Point3.h

@@ -22,7 +22,8 @@ namespace gtsam {
   class Point3: Testable<Point3>, public Lie<Point3> {
   public:
 	  /// dimension of the variable - used to autodetect sizes
-	  static inline size_t dim() {return 3;}
+	  static const size_t dimension = 3;
+
   private:
     double x_, y_, z_;  
 
@@ -38,6 +39,26 @@ namespace gtsam {
     /** equals with an tolerance */
     bool equals(const Point3& p, double tol = 1e-9) const;
 
+    /** dimension of the variable - used to autodetect sizes */
+    inline static size_t Dim() { return dimension; }
+
+    /** Lie requirements */
+
+    /** return DOF, dimensionality of tangent space */
+    inline size_t dim() const { return dimension; }
+
+    /** "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3() */
+    inline Point3 inverse() const { return Point3(-x_, -y_, -z_); }
+
+    /** "Compose" - just adds coordinates of two points */
+    inline Point3 compose(const Point3& p1) const { return *this+p1; }
+
+    /** Exponential map at identity - just create a Point3 from x,y,z */
+    static inline Point3 Expmap(const Vector& v) { return Point3(v); }
+
+    /** Log map at identity - return the x,y,z of this point */
+    static inline Vector Logmap(const Point3& dp) { return Vector_(3, dp.x(), dp.y(), dp.z()); }
+
     /** return vectorized form (column-wise)*/
     Vector vector() const {
       //double r[] = { x_, y_, z_ };
@@ -80,22 +101,7 @@ namespace gtsam {
     }
   };
 
-
-  /** Global print calls member function */
-  inline void print(const Point3& p, const std::string& s) { p.print(s); }
-  inline void print(const Point3& p) { p.print(); }
-
-  /** return DOF, dimensionality of tangent space */
-  inline size_t dim(const Point3&) { return 3; }
-
-  /** Exponential map at identity - just create a Point3 from x,y,z */
-  template<> inline Point3 expmap(const Vector& dp) { return Point3(dp); }
-
-  /** Log map at identity - return the x,y,z of this point */
-  inline Vector logmap(const Point3& dp) { return Vector_(3, dp.x(), dp.y(), dp.z()); }
-
   /** "Compose" - just adds coordinates of two points */
-  inline Point3 compose(const Point3& p1, const Point3& p0) { return p0+p1; }
   inline Matrix Dcompose1(const Point3& p1, const Point3& p0) {
     return Matrix_(3,3,
         1.0, 0.0, 0.0,
@@ -109,10 +115,6 @@ namespace gtsam {
         0.0, 0.0, 1.0);
   }
 
-  /** "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3() */
-  inline Point3 inverse(const Point3& p) { return Point3(-p.x(), -p.y(), -p.z()); }
-
-
   /** Syntactic sugar for multiplying coordinates by a scalar s*p */
   inline Point3 operator*(double s, const Point3& p) { return p*s;}
 

geometry/Pose2.cpp

@@ -68,13 +68,15 @@ namespace gtsam {
 
 #else
 
-	template<> Pose2 expmap(const Vector& v) {
-		return Pose2(v[0], v[1], v[2]);
-	}
+  /* ************************************************************************* */
+  Pose2 Pose2::Expmap(const Vector& v) {
+	  return Pose2(v[0], v[1], v[2]);
+  }
 
-	Vector logmap(const Pose2& p) {
-		return Vector_(3, p.x(), p.y(), p.theta());
-	}
+  /* ************************************************************************* */
+  Vector Pose2::Logmap(const Pose2& p) {
+	  return Vector_(3, p.x(), p.y(), p.theta());
+  }
 
 #endif
 
@@ -93,15 +95,15 @@ namespace gtsam {
 	}
 
   /* ************************************************************************* */
-  Pose2 inverse(const Pose2& pose) {
-		const Rot2& R = pose.r();
-		const Point2& t = pose.t();
-		return Pose2(inverse(R), R.unrotate(Point2(-t.x(), -t.y())));
-	}
+  Pose2 Pose2::inverse() const {
+	  const Rot2& R = r_;
+	  const Point2& t = t_;
+	  return Pose2(R.inverse(), R.unrotate(Point2(-t.x(), -t.y())));
+  }
 
-	Matrix Dinverse(const Pose2& pose) {
-		return -AdjointMap(pose);
-	}
+  Matrix Dinverse(const Pose2& pose) {
+	  return -AdjointMap(pose);
+  }
 
   /* ************************************************************************* */
   // see doc/math.lyx, SE(2) section
@@ -187,15 +189,15 @@ namespace gtsam {
   /* ************************************************************************* */
 	Rot2 bearing(const Pose2& pose, const Point2& point) {
 		Point2 d = transform_to(pose, point);
-		return relativeBearing(d);
+		return Rot2::relativeBearing(d);
 	}
 
 	Rot2 bearing(const Pose2& pose, const Point2& point,
 			boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
 		if (!H1 && !H2) return bearing(pose, point);
 		Point2 d = transform_to(pose, point, H1, H2);
 		Matrix D_result_d;
-		Rot2 result = relativeBearing(d, D_result_d);
+		Rot2 result = Rot2::relativeBearing(d, D_result_d);
 		if (H1) *H1 = D_result_d * (*H1);
 		if (H2) *H2 = D_result_d * (*H2);
 		return result;