Testable concept prereqs check

gtsam/base/Testable.h

@@ -19,7 +19,7 @@
  * The concept checking function will check whether or not
  * the function exists in derived class and throw compile-time errors.
  * 
- * print with optional string naming the t
+ * print with optional string naming the object
  *     void print(const std::string& name) const = 0;
  * 
  * equality up to tolerance
@@ -51,7 +51,7 @@ namespace gtsam {
    *
    * See macros for details on using this structure
    * @addtogroup base
-   * @tparam T is the type this constrains to be testable - assumes print() and equals()
+   * @tparam T is the objectype this constrains to be testable - assumes print() and equals()
    */
   template <class T>
   class IsTestable {
@@ -71,11 +71,6 @@ namespace gtsam {
     }
   }; // \ Testable
 
-  /** Call print on the t */
-  template<class T>
-  inline void print(const T& t, const std::string& s = "") {
-    traits_x<T>::Print(t,s);
-  }
   inline void print(float v, const std::string& s = "") {
     printf("%s%f\n",s.c_str(),v);
   }
@@ -83,7 +78,7 @@ namespace gtsam {
     printf("%s%lf\n",s.c_str(),v);
   }
 
-  /** Call equal on the t */
+  /** Call equal on the object */
   template<class T>
   inline bool equal(const T& obj1, const T& obj2, double tol) {
     return traits_x<T>::Equals(obj1,obj2, tol);
@@ -133,11 +128,30 @@ namespace gtsam {
     }
   };
 
+  /// Requirements on type to pass it to Testable template below
+  template<typename T>
+  struct HasTestablePrereqs {
+
+    BOOST_CONCEPT_USAGE(HasTestablePrereqs) {
+      t->print(str);
+      b = t->equals(*s,tol);
+    }
+
+    T *t, *s; // Pointer is to allow abstract classes
+    bool b;
+    double tol;
+    std::string str;
+  };
+
   /// A helper that implements the traits interface for GTSAM types.
   /// To use this for your gtsam type, define:
   /// template<> struct traits<Type> : public Testable<Type> { };
   template<typename T>
   struct Testable {
+
+    // Check that T has the necessary methods
+    BOOST_CONCEPT_ASSERT((HasTestablePrereqs<T>));
+
     static void Print(const T& m, const std::string& str = "") {
       m.print(str);
     }

gtsam/geometry/SO3.h

@@ -52,6 +52,14 @@ public:
     return I_3x3;
   }
 
+  void print(const std::string& s) const {
+    std::cout << s << *this << std::endl;
+  }
+
+  bool equals(const SO3 & R, double tol) const {
+    return equal_with_abs_tol(*this, R, tol);
+  }
+
 };
 
 template<>

gtsam/inference/FactorGraph.h

@@ -250,7 +250,6 @@ namespace gtsam {
     void print(const std::string& s = "FactorGraph",
       const KeyFormatter& formatter = DefaultKeyFormatter) const;
 
-  protected:
     /** Check equality */
     bool equals(const This& fg, double tol = 1e-9) const;
     /// @}

gtsam_unstable/slam/Mechanization_bRn2.h

@@ -82,8 +82,8 @@ public:
   void print(const std::string& s = "") const {
     bRn_.print(s + ".R");
 
-    gtsam::print(x_g_, s + ".x_g");
-    gtsam::print(x_a_, s + ".x_a");
+    std::cout << s + ".x_g" << x_g_ << std::endl;
+    std::cout << s + ".x_a" << x_a_ << std::endl;
   }
 
 };