Deriving Lie objects from a base class Lie<T>, which provides member functions to access global functions, for use in MATLAB.

cpp/Lie-inl.h

@@ -0,0 +1,32 @@
+/*
+ * LieBaseImplementations.h
+ *
+ *  Created on: Jan 9, 2010
+ *      Author: richard
+ */
+
+#include "Lie.h"
+
+namespace gtsam {
+  template<class T>
+  size_t Lie<T>::dim() const {
+    return gtsam::dim(*((T*)this));
+  }
+
+  /**
+   * Returns Exponential mapy
+   */
+  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);
+  }
+
+}

cpp/Lie.h

@@ -38,6 +38,29 @@ namespace gtsam {
   template<class T>
   inline T expmap(const T& l0, const Vector& v) { return expmap<T>(v)*l0; }
 
+  /**
+   * Base class for Lie group type
+   */
+  template <class T>
+  class Lie {
+  public:
+
+    /**
+     * Returns dimensionality of the tangent space
+     */
+    size_t dim() const;
+
+    /**
+     * Returns Exponential mapy
+     */
+    T expmap(const Vector& v) const;
+
+    /**
+     * Returns Log map
+     */
+    Vector logmap(const T& lp) const;
+
+  };
 }
 
 

cpp/Makefile.am

@@ -157,7 +157,7 @@ testSQPOptimizer_SOURCES   		  = $(example) testSQPOptimizer.cpp
 testSQPOptimizer_LDADD     		  = libgtsam.la
 
 # geometry
-headers += Lie.h
+headers += Lie.h Lie-inl.h
 sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp
 check_PROGRAMS += testPoint2 testRot2 testPose2 testPoint3 testRot3 testPose3 testCal3_S2
 testPoint2_SOURCES  = testPoint2.cpp

cpp/Point2.cpp

@@ -5,11 +5,15 @@
  */
 
 #include "Point2.h"
+#include "Lie-inl.h"
 
 using namespace std;
 
 namespace gtsam {
 
+  /** Explicit instantiation of base class to export members */
+  template class Lie<Point2>;
+
   /* ************************************************************************* */
   void Point2::print(const string& s) const {
     cout << s << "(" << x_ << ", " << y_ << ")" << endl;

cpp/Point2.h

@@ -18,7 +18,7 @@ namespace gtsam {
    * Derived from testable so has standard print and equals, and assert_equals works
    * Functional, so no set functions: once created, a point is constant.
    */
-  class Point2: Testable<Point2> {
+  class Point2: Testable<Point2>, public Lie<Point2> {
   private:
     double x_, y_;
 		

cpp/Point3.cpp

@@ -4,10 +4,13 @@
  */
 
 #include "Point3.h"
+#include "Lie-inl.h"
 
 
 namespace gtsam {
 
+  /** Explicit instantiation of base class to export members */
+  template class Lie<Point3>;
 
   /* ************************************************************************* */
   bool Point3::equals(const Point3 & q, double tol) const {

cpp/Point3.h

@@ -19,7 +19,7 @@
 namespace gtsam {
 
   /** A 3D point */
-  class Point3: Testable<Point3> {
+  class Point3: Testable<Point3>, public Lie<Point3> {
   private:
     double x_, y_, z_;  
 		

cpp/Pose2.cpp

@@ -4,11 +4,15 @@
  */
 
 #include "Pose2.h"
+#include "Lie-inl.h"
 
 using namespace std;
 
 namespace gtsam {
 
+  /** Explicit instantiation of base class to export members */
+  template class Lie<Pose2>;
+
   /* ************************************************************************* */
   void Pose2::print(const string& s) const {
     cout << s << "(" << t_.x() << ", " << t_.y() << ", " << r_.theta() << ")" << endl;

cpp/Pose2.h

@@ -9,18 +9,18 @@
 
 #pragma once
 
-#include "Point2.h"
-#include "Rot2.h"
 #include "Matrix.h"
 #include "Testable.h"
 #include "Lie.h"
+#include "Point2.h"
+#include "Rot2.h"
 
 namespace gtsam {
 
   /**
    * A 2D pose (Point2,Rot2)
    */
-  class Pose2: Testable<Pose2>  {
+  class Pose2: Testable<Pose2>, public Lie<Pose2>  {
   private:
     Point2 t_;
     Rot2 r_;
@@ -73,10 +73,12 @@ namespace gtsam {
   inline Pose2 compose(const Pose2& p1, const Pose2& p0) {
     return Pose2(p0.r()*p1.r(), p0.t() + p0.r()*p1.t()); }
 
-  /* exponential and log maps around identity */
+  /** exponential and log maps around identity */
-  // Create an incremental pose from x,y,theta
+
+  /** Create an incremental pose from x,y,theta */
   template<> inline Pose2 expmap(const Vector& v) { return Pose2(v[0], v[1], v[2]); }
-  // Return the x,y,theta of this pose
+
+  /** Return the x,y,theta of this pose */
   inline Vector logmap(const Pose2& p) { return Vector_(3, p.x(), p.y(), p.theta()); }
 
 
@@ -91,7 +93,7 @@ namespace gtsam {
     return rotate(pose.r(), point)+pose.t(); }
 
   /** Return relative pose between p1 and p2, in p1 coordinate frame */
-  // todo: make sure compiler finds this version of between.
+  /** todo: make sure compiler finds this version of between. */
   //inline Pose2 between(const Pose2& p0, const Pose2& p2) {
   //  return Pose2(p0.r().invcompose(p2.r()), p0.r().unrotate(p2.t()-p0.t())); }
   Matrix Dbetween1(const Pose2& p0, const Pose2& p2);
@@ -105,4 +107,7 @@ namespace gtsam {
   inline Point2 operator*(const Pose2& pose, const Point2& point) {
     return transform_from(pose, point); }
 
+
+
 } // namespace gtsam
+

cpp/Pose3.cpp

@@ -5,12 +5,16 @@
 
 #include <iostream>
 #include "Pose3.h"
+#include "Lie-inl.h"
 
 using namespace std;
 using namespace boost::numeric::ublas;
 
 namespace gtsam {
 
+  /** Explicit instantiation of base class to export members */
+  template class Lie<Pose3>;
+
   /* ************************************************************************* */
   void Pose3::print(const string& s) const {
     R_.print(s + ".R");

cpp/Pose3.h

@@ -17,7 +17,7 @@
 namespace gtsam {
 
   /** A 3D pose (R,t) : (Rot3,Point3) */
-  class Pose3 : Testable<Pose3> {
+  class Pose3 : Testable<Pose3>, public Lie<Pose3> {
   private:
     Rot3 R_;
     Point3 t_;

cpp/Rot2.cpp

@@ -6,11 +6,15 @@
  */
 
 #include "Rot2.h"
+#include "Lie-inl.h"
 
 using namespace std;
 
 namespace gtsam {
 
+  /** Explicit instantiation of base class to export members */
+  template class Lie<Rot2>;
+
   /* ************************************************************************* */
   void Rot2::print(const string& s) const {
     cout << s << ":" << theta() << endl;

cpp/Rot2.h

@@ -16,7 +16,7 @@
 namespace gtsam {
 
   /* Rotation matrix */
-  class Rot2: Testable<Rot2> {
+  class Rot2: Testable<Rot2>, public Lie<Rot2> {
   private:
     /** we store cos(theta) and sin(theta) */
     double c_, s_;

cpp/Rot3.cpp

@@ -7,11 +7,15 @@
  */
 
 #include "Rot3.h"
+#include "Lie-inl.h"
 
 using namespace std;
 
 namespace gtsam {
 
+  /** Explicit instantiation of base class to export members */
+  template class Lie<Rot3>;
+
   /* ************************************************************************* */
 	// static member functions to construct rotations
 

cpp/Rot3.h

@@ -17,7 +17,7 @@
 namespace gtsam {
 
   /* 3D Rotation */
-  class Rot3: Testable<Rot3> {
+  class Rot3: Testable<Rot3>, public Lie<Rot3> {
   private:
     /** we store columns ! */
     Point3 r1_, r2_, r3_;  

cpp/testPose2.cpp

@@ -186,6 +186,13 @@ TEST( Pose2, between )
   CHECK(assert_equal(numericalH2,actualH2));
 }
 
+/* ************************************************************************* */
+TEST(Pose2, members)
+{
+  Pose2 pose;
+  CHECK(pose.dim() == 3);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;