Added concept checks to several generic algorithms

gtsam/inference/EliminationTree.h

@@ -9,6 +9,7 @@
 #include <utility>
 
 #include <gtsam/base/FastSet.h>
+#include <gtsam/base/Testable.h>
 #include <gtsam/inference/VariableIndex.h>
 #include <gtsam/inference/BayesNet.h>
 #include <gtsam/inference/FactorGraph.h>
@@ -48,6 +49,9 @@ public:
 
 private:
 
+  /** concept check */
+  GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
+
   typedef FastList<sharedFactor> Factors;
   typedef FastList<shared_ptr> SubTrees;
   typedef std::vector<typename FACTOR::ConditionalType::shared_ptr> Conditionals;

gtsam/inference/FactorGraph.h

@@ -25,6 +25,7 @@
 #include <boost/serialization/nvp.hpp>
 #include <boost/function.hpp>
 
+#include <gtsam/base/Testable.h>
 #include <gtsam/base/FastMap.h>
 #include <gtsam/inference/BayesNet.h>
 #include <gtsam/inference/graph.h>
@@ -56,6 +57,9 @@ namespace gtsam {
 
 	protected:
 
+	  /** concept check */
+	  GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
+
     /** Collection of factors */
 		std::vector<sharedFactor> factors_;
 

gtsam/inference/GenericSequentialSolver.h

@@ -20,6 +20,7 @@
 
 #include <utility>
 
+#include <gtsam/base/Testable.h>
 #include <gtsam/inference/EliminationTree.h>
 
 namespace gtsam {
@@ -53,8 +54,13 @@ namespace gtsam {
 		VariableIndex::shared_ptr structure_;
 
 		/** Elimination tree that performs elimination */
+		typedef EliminationTree<FACTOR> EliminationTree_;
 		typename EliminationTree<FACTOR>::shared_ptr eliminationTree_;
 
+		/** concept checks */
+		GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
+		GTSAM_CONCEPT_TESTABLE_TYPE(EliminationTree_)
+
 	public:
 
 		/**

gtsam/nonlinear/LieValues.h

@@ -26,6 +26,7 @@
 
 #include <set>
 
+#include <gtsam/base/Testable.h>
 #include <gtsam/base/FastMap.h>
 #include <gtsam/base/Vector.h>
 #include <gtsam/nonlinear/Ordering.h>
@@ -63,6 +64,9 @@ namespace gtsam {
 
   private:
 
+    /** concept check */
+    GTSAM_CONCEPT_TESTABLE_TYPE(Value)
+
     KeyValueMap values_;
 
   public:

gtsam/nonlinear/NonlinearEquality.h

@@ -28,7 +28,10 @@ namespace gtsam {
 	 * Template default compare function that assumes a testable T
 	 */
 	template<class T>
-	bool compare(const T& a, const T& b) { return a.equals(b); }
+	bool compare(const T& a, const T& b) {
+		GTSAM_CONCEPT_TESTABLE_TYPE(T);
+		return a.equals(b);
+	}
 
 	/**
 	 * An equality factor that forces either one variable to a constant,

gtsam/nonlinear/TupleValues.h

@@ -54,6 +54,10 @@ namespace gtsam {
 	  VALUES1 first_;	/// Arbitrary values
 	  VALUES2 second_;	/// A TupleValues or TupleValuesEnd, which wraps an arbitrary values
 
+	  /** concept checks */
+	  GTSAM_CONCEPT_TESTABLE_TYPE(VALUES1)
+	  GTSAM_CONCEPT_TESTABLE_TYPE(VALUES2)
+
   public:
 	  // typedefs for values subtypes
 	  typedef typename VALUES1::Key Key1;

gtsam/slam/simulated2DConstraints.h

@@ -59,6 +59,8 @@ namespace gtsam {
 				typedef boost::shared_ptr<ScalarCoordConstraint1<VALUES, KEY, IDX> > shared_ptr; ///< boost::shared_ptr convenience typedef
 				typedef typename KEY::Value Point; ///< Constrained variable type
 
+				virtual ~ScalarCoordConstraint1() {}
+
 				/**
 				 * Constructor for constraint
 				 * @param key is the label for the constrained variable
@@ -119,6 +121,8 @@ namespace gtsam {
 				typedef BoundingConstraint2<VALUES, KEY, KEY> Base;  ///< Base class for factor
 				typedef typename KEY::Value Point; ///< Type of variable constrained
 
+				virtual ~MaxDistanceConstraint() {}
+
 				/**
 				 * Primary constructor for factor
 				 * @param key1 is the first variable key
@@ -161,6 +165,8 @@ namespace gtsam {
 				typedef typename XKEY::Value Pose; ///< Type of pose variable constrained
 				typedef typename PKEY::Value Point; ///< Type of point variable constrained
 
+				virtual ~MinDistanceConstraint() {}
+
 				/**
 				 * Primary constructor for factor
 				 * @param key1 is the first variable key