diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index 015def233..9bf1a8aa2 100644 --- a/gtsam/base/Testable.h +++ b/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 IsTestable { @@ -71,11 +71,6 @@ namespace gtsam { } }; // \ Testable - /** Call print on the t */ - template - inline void print(const T& t, const std::string& s = "") { - traits_x::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 inline bool equal(const T& obj1, const T& obj2, double tol) { return traits_x::Equals(obj1,obj2, tol); @@ -133,11 +128,30 @@ namespace gtsam { } }; + /// Requirements on type to pass it to Testable template below + template + 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 : public Testable { }; template struct Testable { + + // Check that T has the necessary methods + BOOST_CONCEPT_ASSERT((HasTestablePrereqs)); + static void Print(const T& m, const std::string& str = "") { m.print(str); } diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 64e95547f..964fdc095 100644 --- a/gtsam/geometry/SO3.h +++ b/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<> diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 043c4caf6..adb1c0aad 100644 --- a/gtsam/inference/FactorGraph.h +++ b/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; /// @} diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h index 506f07ea6..fa33ce5b9 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.h +++ b/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; } };