Point2 now Testable, removed obsolete assert_equal, as every testable class can use the template version in Testable.h

cpp/Point2.cpp

@@ -15,25 +15,14 @@ namespace gtsam {
 		cout << s << "(" << x_ << ", " << y_ << ")" << endl;
 	}
 
-	/* ************************************************************************* */
-	double Point2::dist(const Point2& p2) const {
-		return sqrt(pow(x() - p2.x(), 2.0) + pow(y() - p2.y(), 2.0));
-	}
-
 	/* ************************************************************************* */
 	bool Point2::equals(const Point2& q, double tol) const {
 		return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol);
 	}
 
 	/* ************************************************************************* */
-	bool assert_equal(const Point2& p, const Point2& q, double tol) {
+	double Point2::dist(const Point2& p2) const {
-		if (p.equals(q, tol))
+		return sqrt(pow(x() - p2.x(), 2.0) + pow(y() - p2.y(), 2.0));
-			return true;
-		printf("not equal:\n");
-		p.print("p = ");
-		q.print("q = ");
-		(p - q).print("p-q = ");
-		return false;
 	}
 
 /* ************************************************************************* */

cpp/Point2.h

@@ -8,13 +8,18 @@
 
 #include <boost/serialization/nvp.hpp>
 #include "Vector.h"
+#include "Testable.h"
 
 namespace gtsam {
 
-  /** A 2D point */
+  /**
-  class Point2 {
+   * A 2D point
+   * Derived from testable so has standard print and equals, and assert_equals works
+   * Functional, so no set functions: once created, a point is constant.
+   */
+  class Point2: Testable<Point2> {
   private:
-    double x_, y_;  
+    double x_, y_;
 		
   public:
     Point2(): x_(0), y_(0) {}
@@ -22,14 +27,16 @@ namespace gtsam {
     Point2(double x, double y): x_(x), y_(y) {}
     Point2(const Vector& v) : x_(v(0)), y_(v(1)) {}
 
+    /** print with optional string */
+    void print(const std::string& s = "") const;
+
+		/** equals with an tolerance, prints out message if unequal*/
+		bool equals(const Point2& q, double tol = 1e-9) const;
+
     /** get functions for x, y */
     double x() const {return x_;}
     double y() const {return y_;}
 
-    /** set functions for x, y */
-    void set_x(double x) {x_=x;}
-    void set_y(double y) {y_=y;}
-
     /** return DOF, dimensionality of tangent space */
     size_t dim() const { return 2;}
 		
@@ -51,15 +58,9 @@ namespace gtsam {
     inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);}
     inline Point2 operator / (double q) const {return Point2(x_/q,y_/q);}
 
-    /** print with optional string */
-    void print(const std::string& s = "") const;
-    
     /** distance between two points */
     double dist(const Point2& p2) const;
 
-		/** equals with an tolerance, prints out message if unequal*/
-		bool equals(const Point2& q, double tol = 1e-9) const;
-
   private:
     /** Serialization function */
     friend class boost::serialization::access;
@@ -70,8 +71,5 @@ namespace gtsam {
       ar & BOOST_SERIALIZATION_NVP(y_);
     }
   };
-
-  /** equals with an tolerance, prints out message if unequal*/
-  bool assert_equal(const Point2& p, const Point2& q, double tol = 1e-9);
 }