From c6e2547dad139acf6c0f0981aaa5951d7dcb49d5 Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Thu, 6 Oct 2011 20:47:30 +0000
Subject: [PATCH] Added concept checking for "Poses", and made both Pose2 and
 Pose3 comply

---
 gtsam/geometry/Makefile.am |  3 +++
 gtsam/geometry/Pose2.cpp   |  4 ++++
 gtsam/geometry/Pose2.h     | 10 ++++++++++
 gtsam/geometry/Pose3.cpp   | 10 ++++++++--
 gtsam/geometry/Pose3.h     |  4 ++++
 gtsam/geometry/concepts.h  | 41 ++++++++++++++++++++++++++++++++++++++
 6 files changed, 70 insertions(+), 2 deletions(-)
 create mode 100644 gtsam/geometry/concepts.h

diff --git a/gtsam/geometry/Makefile.am b/gtsam/geometry/Makefile.am
index 5f9ba22fc..820922d32 100644
--- a/gtsam/geometry/Makefile.am
+++ b/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
diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
index 9660b86e7..021340ccd 100644
--- a/gtsam/geometry/Pose2.cpp
+++ b/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.));
 
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index cc8f8848d..67e712810 100644
--- a/gtsam/geometry/Pose2.h
+++ b/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;
diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index a0873db48..60571d04f 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/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();
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index 17c61230a..6d90370f1 100644
--- a/gtsam/geometry/Pose3.h
+++ b/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_;
diff --git a/gtsam/geometry/concepts.h b/gtsam/geometry/concepts.h
new file mode 100644
index 000000000..be400f758
--- /dev/null
+++ b/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