made all classes that currently have an assert_equal "Testable"

- derive from testable as in class Point2 : Testable<Point2>
- moved print and equal declarations in .h right after the constructor
- similarly, moved implementations after constructors in .cpp file
- removed obsolete assert_equal

cpp/Cal3_S2.cpp

@@ -43,18 +43,6 @@ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
 
 /* ************************************************************************* */
 
-bool assert_equal(const Cal3_S2& actual, const Cal3_S2& expected, double tol) {
-	bool ret = actual.equals(expected, tol);
-	if (!ret) {
-		cout << "Not Equal:" << endl;
-		actual.print("Actual");
-		expected.print("Expected");
-	}
-	return ret;
-}
-
-/* ************************************************************************* */
-
 Point2 uncalibrate(const Cal3_S2& K, const Point2& p) {
 	return K.uncalibrate(p);
 }

cpp/Cal3_S2.h

@@ -12,7 +12,7 @@
 namespace gtsam {
 
 	/** The most common 5DOF 3D->2D calibration */
-	class Cal3_S2 {
+	class Cal3_S2: Testable<Cal3_S2> {
 	private:
 		double fx_, fy_, s_, u0_, v0_;
 
@@ -31,6 +31,15 @@ namespace gtsam {
 			fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {
 		}
 
+		void print(const std::string& s = "") const {
+			gtsam::print(matrix(), s);
+		}
+
+		/**
+		 * Check if equal up to specified tolerance
+		 */
+		bool equals(const Cal3_S2& K, double tol = 10e-9) const;
+
 		/**
 		 * return DOF, dimensionality of tangent space
 		 */
@@ -82,10 +91,6 @@ namespace gtsam {
 			return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
 		}
 
-		void print(const std::string& s = "") const {
-			gtsam::print(matrix(), s);
-		}
-
 		std::string dump() const {
 			char buffer[100];
 			buffer[0] = 0;
@@ -93,11 +98,6 @@ namespace gtsam {
 			return std::string(buffer);
 		}
 
-		/**
-		 * Check if equal up to specified tolerance
-		 */
-		bool equals(const Cal3_S2& K, double tol = 10e-9) const;
-
 		/** friends */
 		friend Matrix Duncalibrate2(const Cal3_S2& K, const Point2& p);
 
@@ -115,11 +115,6 @@ namespace gtsam {
 		}
 	};
 
-	/**
-	 * assert for unit tests
-	 */
-	bool assert_equal(const Cal3_S2& actual, const Cal3_S2& expected, double tol = 1e-8);
-
 	/**
 	 * convert intrinsic coordinates xy to image coordinates uv
 	 */

cpp/LinearConstraint.cpp

@@ -38,24 +38,6 @@ LinearConstraint::LinearConstraint(const std::map<std::string, Matrix>& matrices
 {
 }
 
-ConstrainedConditionalGaussian::shared_ptr LinearConstraint::eliminate(const std::string& key) {
-	// check to ensure key is one of constraint nodes
-	const_iterator it = As.find(key);
-	if (it == As.end())
-		throw invalid_argument("Node " + key + " is not in LinearConstraint");
-
-	// extract the leading matrix
-	Matrix A1 = it->second;
-
-	// assemble parents
-	map<string, Matrix> parents = As;
-	parents.erase(key);
-
-	// construct resulting CCG with parts
-	ConstrainedConditionalGaussian::shared_ptr ccg(new ConstrainedConditionalGaussian(A1, parents, b));
-	return ccg;
-}
-
 void LinearConstraint::print(const string& s) const {
 	cout << s << ": " << endl;
 	string key; Matrix A;
@@ -85,6 +67,24 @@ bool LinearConstraint::equals(const LinearConstraint& f, double tol) const {
 	return true;
 }
 
+ConstrainedConditionalGaussian::shared_ptr LinearConstraint::eliminate(const std::string& key) {
+	// check to ensure key is one of constraint nodes
+	const_iterator it = As.find(key);
+	if (it == As.end())
+		throw invalid_argument("Node " + key + " is not in LinearConstraint");
+
+	// extract the leading matrix
+	Matrix A1 = it->second;
+
+	// assemble parents
+	map<string, Matrix> parents = As;
+	parents.erase(key);
+
+	// construct resulting CCG with parts
+	ConstrainedConditionalGaussian::shared_ptr ccg(new ConstrainedConditionalGaussian(A1, parents, b));
+	return ccg;
+}
+
 bool LinearConstraint::involves(const std::string& key) const {
 	return As.find(key) != As.end();
 }
@@ -104,17 +104,6 @@ string LinearConstraint::dump() const {
 	return ret;
 }
 
-bool assert_equal(const LinearConstraint& actual,
-		const LinearConstraint& expected, double tol) {
-	bool ret = actual.equals(expected, tol);
-	if (!ret) {
-		cout << "Not Equal:" << endl;
-		actual.print("Actual");
-		expected.print("Expected");
-	}
-	return ret;
-}
-
 LinearConstraint::shared_ptr combineConstraints(
 		const set<LinearConstraint::shared_ptr>& constraints) {
 	map<string, Matrix> blocks;

cpp/LinearConstraint.h

@@ -18,7 +18,7 @@ namespace gtsam {
  * Linear constraints are similar to factors in structure, but represent
  * a different problem
  */
-class LinearConstraint {
+class LinearConstraint: Testable<LinearConstraint> {
 public:
 
 	typedef boost::shared_ptr<LinearConstraint> shared_ptr;
@@ -69,17 +69,6 @@ public:
 	~LinearConstraint() {
 	}
 
-	/**
-	 * Eliminates the constraint
-	 * Note:  Assumes that the constraint will be completely eliminated
-	 * and that the matrix associated with the key is invertible
-	 * @param key is the variable to eliminate
-	 * @return a constrained conditional gaussian for the variable that is a
-	 * function of its parents
-	 */
-	ConstrainedConditionalGaussian::shared_ptr
-	eliminate(const std::string& key);
-
 	/**
 	 * print
 	 * @param s optional string naming the factor
@@ -91,6 +80,16 @@ public:
 	 */
 	bool equals(const LinearConstraint& f, double tol = 1e-9) const;
 
+	/**
+	 * Eliminates the constraint
+	 * Note:  Assumes that the constraint will be completely eliminated
+	 * and that the matrix associated with the key is invertible
+	 * @param key is the variable to eliminate
+	 * @return a constrained conditional gaussian for the variable that is a
+	 * function of its parents
+	 */
+	ConstrainedConditionalGaussian::shared_ptr
+	eliminate(const std::string& key);
 	/**
 	 * returns a version of the factor as a string
 	 */
@@ -138,10 +137,6 @@ public:
  */
 LinearConstraint::shared_ptr combineConstraints(const std::set<LinearConstraint::shared_ptr>& constraints);
 
-/** assert equals for testing - prints when not equal */
-bool assert_equal(const LinearConstraint& actual,
-		const LinearConstraint& expected, double tol = 1e-9);
-
 }
 
 #endif /* EQUALITYFACTOR_H_ */

cpp/Point3.cpp

@@ -8,6 +8,11 @@
 
 namespace gtsam {
 
+/* ************************************************************************* */
+bool Point3::equals(const Point3 & q, double tol) const {
+  return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol);
+}
+
 /* ************************************************************************* */
 
 bool Point3::operator== (const Point3& q) const {
@@ -58,11 +63,6 @@ Matrix Dsub1(const Point3 &p, const Point3 &q) {
 Matrix Dsub2(const Point3 &p, const Point3 &q) {
   return -eye(3,3);
 }
-
-/* ************************************************************************* */
-bool Point3::equals(const Point3 & q, double tol) const {
-  return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol);
-}
 /* ************************************************************************* */
 Point3 cross(const Point3 &p, const Point3 &q)
 {
@@ -81,14 +81,5 @@ double norm(const Point3 &p)
   return sqrt( p.x_*p.x_ + p.y_*p.y_ + p.z_*p.z_ );
 }
 /* ************************************************************************* */
-bool assert_equal(const Point3& p, const Point3& q, double tol) {
-  if(p.equals(q,tol)) return true;
-  printf("not equal:\n");
-  p.print("p = ");
-  q.print("q = ");
-  (p-q).print("p-q = ");
-  return false;
-}
-/* ************************************************************************* */
 
 } // namespace gtsam

cpp/Point3.h

@@ -13,11 +13,12 @@
 #include <boost/serialization/nvp.hpp>
 
 #include "Matrix.h"
+#include "Testable.h"
 
 namespace gtsam {
 
   /** A 3D point */
-  class Point3 {
+  class Point3: Testable<Point3> {
   private:
     double x_, y_, z_;  
 		
@@ -27,6 +28,14 @@ namespace gtsam {
     Point3(double x, double y, double z): x_(x), y_(y), z_(z) {}
     Point3(const Vector& v) : x_(v(0)), y_(v(1)), z_(v(2)) {}
 
+    /** print with optional string */
+    void print(const std::string& s = "") const {
+      std::cout << s << "(" << x_ << ", " << y_ <<  ", " << z_ << ")" << std::endl;
+    }
+
+    /** equals with an tolerance */
+    bool equals(const Point3& p, double tol = 1e-9) const;
+
     /** return DOF, dimensionality of tangent space */
     size_t dim() const { return 3;}
 		
@@ -53,19 +62,11 @@ namespace gtsam {
     Point3 operator * (double s) const;
     Point3 operator / (double s) const;
 
-    /** print with optional string */
-    void print(const std::string& s = "") const {
-      std::cout << s << "(" << x_ << ", " << y_ <<  ", " << z_ << ")" << std::endl;
-    }
-
     /** distance between two points */
     double dist(const Point3& p2) const {
       return sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0) + pow(z()-p2.z(),2.0));
     }
 
-    /** equals with an tolerance */
-    bool equals(const Point3& p, double tol = 1e-9) const;
-
     /** friends */
     friend Point3 cross(const Point3 &p1, const Point3 &p2);
     friend double dot(const Point3 &p1, const Point3 &p2);
@@ -103,7 +104,4 @@ namespace gtsam {
 
   /** dot product */
   double norm(const Point3 &p);
-
-  /** equals with an tolerance, prints out message if unequal */
-  bool assert_equal(const Point3& p, const Point3& q, double tol = 1e-9);
 }

cpp/Pose2.cpp

@@ -14,11 +14,6 @@ namespace gtsam {
 		cout << s << "(" << x_ << ", " << y_ << ", " << theta_ << ")" << endl;
 	}
 
-	/* ************************************************************************* */
-	Pose2 Pose2::exmap(const Vector& v) const {
-		return Pose2(x_ + v(0), y_ + v(1), theta_ + v(2));
-	}
-
 	/* ************************************************************************* */
 	bool Pose2::equals(const Pose2& q, double tol) const {
 		return (fabs(x_ - q.x_) < tol && fabs(y_ - q.y_) < tol && fabs(theta_
@@ -26,12 +21,8 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	bool assert_equal(const Pose2& A, const Pose2& B, double tol) {
+	Pose2 Pose2::exmap(const Vector& v) const {
-		if (A.equals(B, tol)) return true;
+		return Pose2(x_ + v(0), y_ + v(1), theta_ + v(2));
-		printf("not equal:\n");
-		A.print("A");
-		B.print("B");
-		return false;
 	}
 
 /* ************************************************************************* */

cpp/Pose2.h

@@ -9,13 +9,14 @@
 #pragma once
 
 #include "Point2.h"
+#include "Testable.h"
 
 namespace gtsam {
 
 	/**
 	 * A 2D pose (x,y,theta)
 	 */
-	class Pose2 {
+	class Pose2: Testable<Pose2>  {
 
 	private:
 		double x_, y_, theta_;
@@ -47,21 +48,18 @@ namespace gtsam {
 			x_(t.x()), y_(t.y()), theta_(theta) {
 		}
 
+		/** print with optional string */
+		void print(const std::string& s = "") const;
+
+		/** assert equality up to a tolerance */
+		bool equals(const Pose2& pose, double tol = 1e-9) const;
+
     /** get functions for x, y, theta */
 		double x()     const { return x_;}
 		double y()     const { return y_;}
 		double theta() const { return theta_;}
 
-		/** print with optional string */
-		void print(const std::string& s = "") const;
-
 		Pose2 exmap(const Vector& v) const;
-
-		/** assert equality up to a tolerance */
-		bool equals(const Pose2& pose, double tol = 1e-9) const;
 	};
 
-	/** assert equality up to a tolerance */
-	bool assert_equal(const Pose2& A, const Pose2& B, double tol = 1e-9);
-
 } // namespace gtsam

cpp/Pose3.cpp

@@ -9,6 +9,18 @@ using namespace std;
 
 namespace gtsam {
 
+/* ************************************************************************* */
+void Pose3::print(const string& s) const {
+	R_.print(s + ".R");
+	t_.print(s + ".t");
+}
+
+/* ************************************************************************* */
+bool Pose3::equals(const Pose3& pose, double tol) const
+{
+  return R_.equals(pose.R_,tol) && t_.equals(pose.t_,tol);
+}
+
 /* ************************************************************************* */
 // Agrawal06iros, formula (6), seems to suggest this could be wrong:
 Pose3 Pose3::exmap(const Vector& v) const { 
@@ -28,12 +40,6 @@ Matrix Pose3::matrix() const {
 	return stack(2, &A34, &A14);
 }
 
-/* ************************************************************************* */
-void Pose3::print(const string& s) const {
-	R_.print(s + ".R");
-	t_.print(s + ".t");
-}
-
 /* ************************************************************************* */
 Point3 transform_from(const Pose3& pose, const Point3& p) {
   return pose.R_ * p + pose.t_;
@@ -106,22 +112,6 @@ Pose3 Pose3::inverse() const
   return Pose3(Rt,-(Rt*t_));
 }
 
-/* ************************************************************************* */
-bool Pose3::equals(const Pose3& pose, double tol) const
-{
-  return R_.equals(pose.R_,tol) && t_.equals(pose.t_,tol);
-}
-
-/* ************************************************************************* */
-bool assert_equal(const Pose3& A, const Pose3& B, double tol)
-{
-  if(A.equals(B,tol)) return true;
-  printf("not equal:\n");
-  A.print("A");
-  B.print("B");
-  return false;
-}
-
 /* ************************************************************************* */
 Pose3 Pose3::transformPose_to(const Pose3& pose) const
 {

cpp/Pose3.h

@@ -9,11 +9,12 @@
 
 #include "Point3.h"
 #include "Rot3.h"
+#include "Testable.h"
 
 namespace gtsam {
 
 	/** A 3D pose (R,t) : (Rot3,Point3) */
-	class Pose3 {
+	class Pose3 : Testable<Pose3> {
 	private:
 		Rot3 R_;
 		Point3 t_;
@@ -51,6 +52,12 @@ namespace gtsam {
 			t_(V(9), V(10),V(11)) {
 		}
 
+		/** print with optional string */
+		void print(const std::string& s = "") const;
+
+		/** assert equality up to a tolerance */
+		bool equals(const Pose3& pose, double tol = 1e-9) const;
+
 		const Rot3& rotation() const {
 			return R_;
 		}
@@ -81,15 +88,9 @@ namespace gtsam {
 		/** convert to 4*4 matrix */
 		Matrix matrix() const;
 
-		/** print with optional string */
-		void print(const std::string& s = "") const;
-
 		/** transforms */
 		Pose3 transformPose_to(const Pose3& transform) const;
 
-		/** assert equality up to a tolerance */
-		bool equals(const Pose3& pose, double tol = 1e-9) const;
-
 		/** friends */
 		friend Point3 transform_from(const Pose3& pose, const Point3& p);
 		friend Point3 transform_to(const Pose3& pose, const Point3& p);
@@ -131,7 +132,4 @@ namespace gtsam {
 	 */
 	Matrix DhPose(const Vector& x);
 
-	/** assert equality up to a tolerance */
-	bool assert_equal(const Pose3& A, const Pose3& B, double tol = 1e-9);
-
 } // namespace gtsam

cpp/Rot3.cpp

@@ -12,6 +12,11 @@ using namespace std;
 
 namespace gtsam {
 
+	/* ************************************************************************* */
+	bool Rot3::equals(const Rot3 & R, double tol) const {
+		return equal_with_abs_tol(matrix(), R.matrix(), tol);
+	}
+
 	/* ************************************************************************* */
 	/** faster than below ?                                                      */
 	/* ************************************************************************* */
@@ -77,11 +82,6 @@ namespace gtsam {
 		return R.unrotate(p);
 	}
 
-	/* ************************************************************************* */
-	bool Rot3::equals(const Rot3 & R, double tol) const {
-		return equal_with_abs_tol(matrix(), R.matrix(), tol);
-	}
-
 	/* ************************************************************************* */
 	/** see libraries/caml/geometry/math.lyx, derivative of unrotate              */
 	/* ************************************************************************* */
@@ -95,15 +95,6 @@ namespace gtsam {
 		return R.transpose();
 	}
 
-	/* ************************************************************************* */
-	bool assert_equal(const Rot3 & A, const Rot3 & B, double tol) {
-	  if(A.equals(B,tol)) return true;
-	  printf("not equal:\n");
-	  A.print("A");
-	  B.print("B");
-	  return false;
-	}
-
 	/* ************************************************************************* */
 	/** This function receives a rotation 3 by 3 matrix and returns 3 rotation angles.
 	 *  The implementation is based on the algorithm in multiple view geometry

cpp/Rot3.h

@@ -11,11 +11,12 @@
 #pragma once
 
 #include "Point3.h"
+#include "Testable.h"
 
 namespace gtsam {
 
   /* Rotation matrix */
-  class Rot3{
+  class Rot3: Testable<Rot3> {
   private:
     /** we store columns ! */
     Point3 r1_, r2_, r3_;  
@@ -54,6 +55,12 @@ namespace gtsam {
       r2_(Point3(R(0,1), R(1,1), R(2,1))),
       r3_(Point3(R(0,2), R(1,2), R(2,2))) {}
 
+  /** print */
+  void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
+
+  /** equals with an tolerance */
+  bool equals(const Rot3& p, double tol = 1e-9) const;
+
     /** return DOF, dimensionality of tangent space */
     size_t dim() const { return 3;}
 		
@@ -121,12 +128,6 @@ namespace gtsam {
 					);
 		}
 
-    /** print */
-    void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
-
-    /** equals with an tolerance */
-    bool equals(const Rot3& p, double tol = 1e-9) const;
-
     /** friends */
     friend Matrix Dunrotate1(const Rot3& R, const Point3& p);
 
@@ -190,8 +191,6 @@ namespace gtsam {
   Matrix Dunrotate1(const Rot3& R, const Point3& p);
   Matrix Dunrotate2(const Rot3& R); // does not depend on p !
 
-  bool assert_equal(const Rot3& A, const Rot3& B, double tol=1e-9);
-
   /** receives a rotation 3 by 3 matrix and returns 3 rotation angles.*/
   Vector RQ(Matrix R);