Added concept checking for "Poses", and made both Pose2 and Pose3 comply

gtsam/geometry/Makefile.am

@@ -10,6 +10,9 @@ headers =
 sources = 
 check_PROGRAMS = 
 
+# Concept check
+headers += concepts.h
+
 # Points and poses
 sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp 
 check_PROGRAMS += tests/testPoint2 tests/testRot2 tests/testPose2 tests/testPoint3 tests/testRot3 tests/testPose3

gtsam/geometry/Pose2.cpp

@@ -17,6 +17,7 @@
 #include <boost/foreach.hpp>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/base/Lie-inl.h>
+#include <gtsam/geometry/concepts.h>
 
 using namespace std;
 
@@ -25,6 +26,9 @@ namespace gtsam {
   /** Explicit instantiation of base class to export members */
   INSTANTIATE_LIE(Pose2);
 
+  /** instantiate concept checks */
+  GTSAM_CONCEPT_POSE(Pose2);
+
 	static const Matrix I3 = eye(3), Z12 = zeros(1,2);
   static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.));
 

gtsam/geometry/Pose2.h

@@ -35,6 +35,11 @@ namespace gtsam {
 
   public:
 	  static const size_t dimension = 3;
+
+    /** Pose Concept requirements */
+    typedef Rot2 Rotation;
+    typedef Point2 Translation;
+
   private:
     Rot2 r_;
     Point2 t_;
@@ -213,9 +218,14 @@ namespace gtsam {
     inline double y()     const { return t_.y(); }
     inline double theta() const { return r_.theta(); }
 
+    /** shorthand access functions */
     inline const Point2& t() const { return t_; }
     inline const Rot2&   r() const { return r_; }
 
+    /** full access functions required by Pose concept */
+    inline const Point2& translation() const { return t_; }
+    inline const Rot2&   rotation() const { return r_; }
+
   private:
     // Serialization function
     friend class boost::serialization::access;

gtsam/geometry/Pose3.cpp

@@ -17,6 +17,7 @@
 #include <iostream>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/Lie-inl.h>
+#include <gtsam/geometry/concepts.h>
 
 using namespace std;
 
@@ -25,7 +26,13 @@ namespace gtsam {
   /** Explicit instantiation of base class to export members */
   INSTANTIATE_LIE(Pose3);
 
-  static const Matrix I3 = eye(3), _I3=-I3, I6 = eye(6), Z3 = zeros(3, 3);
+  /** instantiate concept checks */
+  GTSAM_CONCEPT_POSE(Pose3);
+
+  static const Matrix I3 = eye(3), Z3 = zeros(3, 3);
+#ifdef CORRECT_POSE3_EXMAP
+  static const _I3=-I3, I6 = eye(6);
+#endif
 
   /* ************************************************************************* */
   // Calculate Adjoint map
@@ -216,7 +223,6 @@ namespace gtsam {
 	  }
 	  if (H2) {
 #ifdef CORRECT_POSE3_EXMAP
-
 		*H2 = I6;
 #else
 		Matrix R1 = rotation().matrix();

gtsam/geometry/Pose3.h

@@ -31,6 +31,10 @@ namespace gtsam {
   public:
 	  static const size_t dimension = 6;
 
+	  /** Pose Concept requirements */
+	  typedef Rot3 Rotation;
+	  typedef Point3 Translation;
+
   private:
     Rot3 R_;
     Point3 t_;

gtsam/geometry/concepts.h

@@ -0,0 +1,41 @@
+/**
+ * @file concepts.h
+ *
+ * @brief Concept-checking macros for geometric objects
+ * Each macro instantiates a concept check structure, which
+ * includes a static function that will fail to compile
+ * if the concept does not pass. Functions are made static
+ * to ensure they get instantiated.
+ *
+ * @date Oct 6, 2011
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+namespace gtsam {
+
+/**
+ * Pose Concept
+ * A must contain a translation and a rotation, with
+ * each structure accessable directly and a type provided
+ * for each.
+ */
+template<class POSE>
+struct PoseConcept {
+	typedef typename POSE::Translation Translation;
+	typedef typename POSE::Rotation Rotation;
+
+	static Rotation checkRotationMemberAccess(const POSE& p) {
+		return p.rotation();
+	}
+
+	static Translation checkTranslationMemberAccess(const POSE& p) {
+		return p.translation();
+	}
+};
+
+/** Instantiation macro */
+#define GTSAM_CONCEPT_POSE(T) template class PoseConcept<T>;
+
+} // \namespace gtsam