Added a "range" concept check for a measurement type - trying to add to generic factor types, but does not compile

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

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 */

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 */