Removed remaining global lie functions on lie objects and configs, sw…

…itched the Lie base class to a simple concept check function, fixed build script for examples. ISAM2 and MastSLAM verified as compiling.

Makefile.am

@@ -11,7 +11,7 @@ ACLOCAL_AMFLAGS = -I m4
 AUTOMAKE_OPTIONS = foreign nostdinc
 
 # All the sub-directories that need to be built
-SUBDIRS = CppUnitLite colamd spqr_mini base geometry inference linear nonlinear slam . tests wrap  
+SUBDIRS = CppUnitLite colamd spqr_mini base geometry inference linear nonlinear slam . tests wrap examples 
 
 # And the corresponding libraries produced
 SUBLIBS = colamd/libcolamd.la \

base/Lie-inl.h

@@ -10,29 +10,10 @@
 #include <gtsam/base/Lie.h>
 
 #define INSTANTIATE_LIE(T) \
-  template T between(const T&, const T&); \
-  template Vector logmap(const T&, const T&); \
-  template T expmap(const T&, const Vector&); \
+  template T between_default(const T&, const T&); \
+  template Vector logmap_default(const T&, const T&); \
+  template T expmap_default(const T&, const Vector&); \
   template bool equal(const T&, const T&, double); \
   template bool equal(const T&, const T&); \
   template class Lie<T>;
 
-namespace gtsam {
-  /**
-   * Returns Exponential mapy
-   * This is for matlab wrapper
-   */
-  template<class T>
-  T Lie<T>::expmap(const Vector& v) const {
-    return gtsam::expmap(*((T*)this),v);
-  }
-
-  /**
-   * Returns Log map
-   */
-  template<class T>
-  Vector Lie<T>::logmap(const T& lp) const {
-    return gtsam::logmap(*((T*)this),lp);
-  }
-
-}

base/Lie.h

@@ -18,102 +18,118 @@ namespace gtsam {
    * for better performance.
    */
 
-  /* Exponential map about identity */
+  /** Compute l0 s.t. l2=l1*l0 */
   template<class T>
-  T expmap(const Vector& v) { return T::Expmap(v); }
-
-  /* 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 */
-  template<class T>
-  inline T between(const T& l1, const T& l2) { return compose(inverse(l1),l2); }
+  inline T between_default(const T& l1, const T& l2) { return l1.inverse().compose(l2); }
 
   /** 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)); }
+  inline Vector logmap_default(const T& l0, const T& lp) { return T::Logmap(l0.between(lp)); }
 
   /** 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)); }
+  inline T expmap_default(const T& t, const Vector& d) { return t.compose(T::Expmap(d)); }
 
   /**
    * Base class for Lie group type
-   * This class uses the Curiously Recurring Template design pattern to allow
-   * for static polymorphism.
+   * This class uses the Curiously Recurring Template design pattern to allow for
+   * concept checking using a private function.
    *
    * 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 {
-  public:
-
-    /**
-     * Returns dimensionality of the tangent space
-     */
-    inline size_t dim() const {
-    	return static_cast<const T*>(this)->dim();
-    }
-
-    /**
-     * 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();
-    }
+  private:
 
-  };
-
-  /** get the dimension of an object with a global function */
-  template<class T>
-  inline size_t dim(const T& object) {
-	  return object.dim();
-  }
+	  /** concept checking function - implement the functions this demands */
+	  static void concept_check(const T& t) {
 
-  /** compose two Lie types */
-  template<class T>
-  inline T compose(const T& p1, const T& p2) {
-	  return p1.compose(p2);
-  }
+		  /** assignment */
+		  T t2 = t;
 
-  /** invert an object */
-  template<class T>
-  inline T inverse(const T& p) {
-	  return p.inverse();
-  }
+		  /**
+		   * Returns dimensionality of the tangent space
+		   */
+		  size_t dim_ret = t.dim();
+
+		  /**
+		   * Returns Exponential map update of T
+		   * Default implementation calls global binary function
+		   */
+		  T expmap_ret = t.expmap(gtsam::zero(dim_ret));
+
+		  /** expmap around identity */
+		  T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret));
+
+		  /**
+		   * Returns Log map
+		   * Default Implementation calls global binary function
+		   */
+		  Vector logmap_ret = t.logmap(t2);
 
+		  /** Logmap around identity */
+		  Vector logmap_identity_ret = T::Logmap(t);
+
+		  /** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */
+		  T between_ret = t.between(t2);
+
+		  /** compose with another object */
+		  T compose_ret = t.compose(t2);
+
+		  /** invert the object and yield a new one */
+		  T inverse_ret = t.inverse();
+	  }
+
+	  /**
+	   * The necessary functions to implement for Lie are defined
+	   * below with additional details as to the interface.  The
+	   * concept checking function above will check whether or not
+	   * the function exists and throw compile-time errors.
+	   */
+
+
+	  /**
+	   * Returns dimensionality of the tangent space
+	   */
+//	  inline size_t dim() const;
+
+	  /**
+	   * Returns Exponential map update of T
+	   * A default implementation of expmap(*this, lp) is available:
+	   * expmap_default()
+	   */
+//	  T expmap(const Vector& v) const;
+
+	  /** expmap around identity */
+//	  static T Expmap(const Vector& v);
+
+	  /**
+	   * Returns Log map
+	   * A default implementation of logmap(*this, lp) is available:
+	   * logmap_default()
+	   */
+//	  Vector logmap(const T& lp) const;
+
+	  /** Logmap around identity */
+//	  static Vector Logmap(const T& p);
+
+	  /**
+	   * Compute l0 s.t. l2=l1*l0, where (*this) is l1
+	   * A default implementation of between(*this, lp) is available:
+	   * between_default()
+	   */
+//	  T between(const T& l2) const;
+
+	  /** compose with another object */
+//	  inline T compose(const T& p) const;
+
+	  /** invert the object and yield a new one */
+//	  inline T inverse() const;
+
+  };
+
   /** Call print on the object */
   template<class T>
   inline void print(const T& object, const std::string& s = "") {

base/LieVector.cpp

@@ -0,0 +1,33 @@
+/**
+ * @file LieVector.cpp
+ * @brief Implementations for LieVector functions
+ * @author Alex Cunningham
+ */
+
+#include <stdarg.h>
+#include <gtsam/base/LieVector.h>
+
+using namespace std;
+
+namespace gtsam {
+
+/* ************************************************************************* */
+LieVector::LieVector(size_t m, const double* const data)
+: Vector(Vector_(m,data))
+{
+}
+
+/* ************************************************************************* */
+LieVector::LieVector(size_t m, ...)
+: Vector(m)
+{
+    va_list ap;
+    va_start(ap, m);
+    for( size_t i = 0 ; i < m ; i++) {
+      double value = va_arg(ap, double);
+      (*this)(i) = value;
+    }
+    va_end(ap);
+}
+
+} // \namespace gtsam

base/LieVector.h

@@ -23,6 +23,15 @@ namespace gtsam {
 		/** initialize from a normal vector */
 		LieVector(const Vector& v) : Vector(v) {}
 
+		/** wrap a double */
+		LieVector(double d) : Vector(Vector_(1, d)) {}
+
+		/** constructor with size and initial data, row order ! */
+		LieVector(size_t m, const double* const data);
+
+		/** Specify arguments directly, as in Vector_() - always force these to be doubles */
+		LieVector(size_t m, ...);
+
 		/** get the underlying vector */
 		inline Vector vector() const {
 			return static_cast<Vector>(*this);
@@ -47,25 +56,33 @@ namespace gtsam {
 	     * Returns Exponential map update of T
 	     * Default implementation calls global binary function
 	     */
-	    LieVector expmap(const Vector& v) const { return LieVector(vector() + v); }
+	    inline LieVector expmap(const Vector& v) const { return LieVector(vector() + v); }
 
 	    /** expmap around identity */
-	    static LieVector Expmap(const Vector& v) { return LieVector(v); }
+	    static inline LieVector Expmap(const Vector& v) { return LieVector(v); }
 
 	    /**
 	     * Returns Log map
 	     * Default Implementation calls global binary function
 	     */
-	    Vector logmap(const LieVector& lp) const { return *this - lp; }
+	    inline Vector logmap(const LieVector& lp) const {
+//	    	return Logmap(between(lp)); // works
+	    	return lp.vector() - vector();
+	    }
 
 	    /** Logmap around identity - just returns with default cast back */
-	    static Vector Logmap(const LieVector& p) { return p; }
+	    static inline Vector Logmap(const LieVector& p) { return p; }
 
 	    /** compose with another object */
 	    inline LieVector compose(const LieVector& p) const {
 	    	return LieVector(vector() + p);
 	    }
 
+	    /** between operation */
+	    inline LieVector between(const LieVector& l2) const {
+	    	return LieVector(l2.vector() - vector());
+	    }
+
 	    /** invert the object and yield a new one */
 	    inline LieVector inverse() const {
 	    	return LieVector(-1.0 * vector());

base/Makefile.am

@@ -26,7 +26,8 @@ endif
 headers += Testable.h TestableAssertions.h numericalDerivative.h 
 
 # Lie Groups
-headers += Lie.h Lie-inl.h LieVector.h
+headers += Lie.h Lie-inl.h lieProxies.h
+sources += LieVector.cpp
 check_PROGRAMS += tests/testLieVector
 
 # Data structures

base/lieProxies.h

@@ -0,0 +1,48 @@
+/**
+ * @file lieProxies.h
+ * @brief Provides convenient mappings of common member functions for testing
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+/**
+ * Global functions in a separate testing namespace
+ *
+ * These should not be used outside of tests, as they are just remappings
+ * of the original functions.  We use these to avoid needing to do
+ * too much boost::bind magic or writing a bunch of separate proxy functions.
+ *
+ * Don't expect all classes to work for all of these functions.
+ */
+namespace gtsam {
+namespace testing {
+
+	/** binary functions */
+	template<class T>
+	T between(const T& t1, const T& t2) { return t1.between(t2); }
+
+	template<class T>
+	T compose(const T& t1, const T& t2) { return t1.compose(t2); }
+
+	/** expmap and logmap */
+	template<class T>
+	Vector logmap(const T& t1, const T& t2) { return t1.logmap(t2); }
+
+	template<class T>
+	T expmap(const T& t1, const Vector& t2) { return t1.expmap(t2); }
+
+	/** unary functions */
+	template<class T>
+	T inverse(const T& t) { return t.inverse(); }
+
+	/** rotation functions */
+	template<class T, class P>
+	P rotate(const T& r, const P& pt) { return r.rotate(pt); }
+
+	template<class T, class P>
+	P unrotate(const T& r, const P& pt) { return r.unrotate(pt); }
+
+}
+}
+