From 0cb5c258df48266ced98dfebecb516b0fc74112f Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Sat, 15 Oct 2011 18:38:44 +0000
Subject: [PATCH] Added a "range" concept check for a measurement type - trying
 to add to generic factor types, but does not compile

---
 gtsam/geometry/concepts.h       | 21 +++++++++++++++++++++
 gtsam/slam/BearingRangeFactor.h |  4 ++++
 gtsam/slam/RangeFactor.h        |  7 +++++++
 3 files changed, 32 insertions(+)

diff --git a/gtsam/geometry/concepts.h b/gtsam/geometry/concepts.h
index be400f758..084cd4ffd 100644
--- a/gtsam/geometry/concepts.h
+++ b/gtsam/geometry/concepts.h
@@ -38,4 +38,25 @@ struct PoseConcept {
 /** Instantiation macro */
 #define GTSAM_CONCEPT_POSE(T) template class PoseConcept<T>;
 
+/**
+ * Range measurement concept
+ * Given a pair of Lie variables, there must exist a function to calculate
+ * range with derivatives.
+ */
+template<class V1, class V2>
+struct RangeMeasurementConcept {
+	static double checkRangeMeasurement(const V1& x, const V2& p) {
+		return x.range(p);
+	}
+
+	static double checkRangeMeasurementDerivatives(const V1& x, const V2& p) {
+		boost::optional<Matrix&> H1, H2;
+		// FIXME: verify the dimensions of the derivative functions
+		return x.range(p, H1, H2);
+	}
+};
+
+/** Instantiation macro */
+#define GTSAM_CONCEPT_RANGE_MEASUREMENT(V1,V2) template class RangeMeasurementConcept<V1,V2>;
+
 } // \namespace gtsam
diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h
index 59dd93ee1..287c6661c 100644
--- a/gtsam/slam/BearingRangeFactor.h
+++ b/gtsam/slam/BearingRangeFactor.h
@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <gtsam/geometry/concepts.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
 namespace gtsam {
@@ -40,6 +41,9 @@ namespace gtsam {
 		Rot bearing_;
 		double range_;
 
+		// Concept requirements for this factor
+		GTSAM_CONCEPT_RANGE_MEASUREMENT(Pose, Point)
+
 	public:
 
 		BearingRangeFactor() {} /* Default constructor */
diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h
index dc91b529b..afa17cb19 100644
--- a/gtsam/slam/RangeFactor.h
+++ b/gtsam/slam/RangeFactor.h
@@ -17,6 +17,7 @@
 #pragma once
 
 #include <boost/lexical_cast.hpp>
+#include <gtsam/geometry/concepts.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
 namespace gtsam {
@@ -33,6 +34,12 @@ namespace gtsam {
 		typedef RangeFactor<VALUES, POSEKEY, POINTKEY> This;
 		typedef NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base;
 
+		typedef typename POSEKEY::Value Pose;
+		typedef typename POINTKEY::Value Point;
+
+		// Concept requirements for this factor
+		GTSAM_CONCEPT_RANGE_MEASUREMENT(Pose, Point)
+
 	public:
 
 		RangeFactor() {} /* Default constructor */