Closing # 118: Made Pose2 use Rot2, added 'compose' for Pose2, made slight modifications to Rot2. Also added additional unit tests. This change shouldn't break anything but let me know if it does.

cpp/Point2.h

@@ -40,7 +40,7 @@ namespace gtsam {
     /** return DOF, dimensionality of tangent space */
     size_t dim() const { return 2;}
 		
-    /** Given 3-dim tangent vector, create new rotation */
+    /** Given 2-dim tangent vector, create new point */
     Point2 exmap(const Vector& d) const { 
       return Point2(x_+d(0),y_+d(1));
     }

cpp/Pose2.cpp

@@ -11,88 +11,66 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	void Pose2::print(const string& s) const {
-		cout << s << "(" << x_ << ", " << y_ << ", " << theta_ << ")" << endl;
+		cout << s << "(" << t_.x() << ", " << t_.y() << ", " << r_.theta() << ")" << endl;
 	}
 
 	/* ************************************************************************* */
 	bool Pose2::equals(const Pose2& q, double tol) const {
-		return (fabs(x_ - q.x_) < tol && fabs(y_ - q.y_) < tol && fabs(theta_
+		return t_.equals(q.t_, tol) && r_.equals(q.r_, tol);
-				- q.theta_) < tol);
 	}
 
 	/* ************************************************************************* */
 	Pose2 Pose2::exmap(const Vector& v) const {
-		return Pose2(x_ + v(0), y_ + v(1), theta_ + v(2));
+	  return Pose2(t_+Point2(v(0),v(1)), r_.exmap(Vector_(1, v(2))));
 	}
 
 	/* ************************************************************************* */
 	Vector Pose2::vector() const {
-		Vector v(3);
+		return Vector_(3, t_.x(), t_.y(), r_.theta());
-		v(0) = x_;
-		v(1) = y_;
-		v(2) = theta_;
-		return v;
 	}
 
 	/* ************************************************************************* */
-	Pose2 Pose2::rotate(double theta) const {
+//	Pose2 Pose2::rotate(double theta) const {
-		double c = cos(theta), s = sin(theta);
+//		//return Pose2(t_, Rot2(theta)*r_);
-		return Pose2(c * x_ - s * y_, s * x_ + c * y_, theta + theta_);
+//	  return Pose2(Point2(0.0,0.0),-theta)*(*this);
-	}
+//	}
 
 	/* ************************************************************************* */
 	Point2 transform_to(const Pose2& pose, const Point2& point) {
-		double dx = point.x()-pose.x(), dy = point.y()-pose.y();
+	  return pose*point;
-		double ct=cos(pose.theta()), st=sin(pose.theta());
-		return Point2(ct*dx + st*dy, -st*dx + ct*dy);
 	}
 
 	// TODO, have a combined function that returns both function value and derivative
 	Matrix Dtransform_to1(const Pose2& pose, const Point2& point) {
-		double dx = point.x()-pose.x(), dy = point.y()-pose.y();
+	  Matrix dx_dt = pose.r().negtranspose();
-		double ct=cos(pose.theta()), st=sin(pose.theta());
+	  Matrix dx_dr = Dunrotate1(pose.r(), point-pose.t());
-		double transformed_x = ct*dx + st*dy, transformed_y = -st*dx + ct*dy;
+	  return collect(2, &dx_dt, &dx_dr);
-		return Matrix_(2,3,
-				-ct, -st,  transformed_y,
-				 st, -ct, -transformed_x
-				);
 	}
 
 	Matrix Dtransform_to2(const Pose2& pose, const Point2& point) {
-		double ct=cos(pose.theta()), st=sin(pose.theta());
+	  return pose.r().transpose();
-		return Matrix_(2,2,
-				 ct,  st,
-				-st,  ct
-				);
 	}
 
 	/* ************************************************************************* */
 	Pose2 between(const Pose2& p1, const Pose2& p2) {
-		double dx = p2.x()-p1.x(), dy = p2.y()-p1.y();
+		return Pose2(
-		double ct=cos(p1.theta()), st=sin(p1.theta());
+		    p1.r().invcompose(p2.r()),
-		return Pose2(ct*dx + st*dy, -st*dx + ct*dy, p2.theta()-p1.theta());
+		    p1.r().unrotate(p2.t() - p1.t()));
 	}
 
 	Matrix Dbetween1(const Pose2& p1, const Pose2& p2) {
-		double dx = p2.x()-p1.x(), dy = p2.y()-p1.y();
+	  Matrix dbt_dp = Dtransform_to1(p1, p2.t());
-		double ct=cos(p1.theta()), st=sin(p1.theta());
+	  Matrix dbr_dp = Matrix_(1,3, 0.0, 0.0, -1.0);
-		double transformed_x = ct*dx + st*dy, transformed_y = -st*dx + ct*dy;
+	  return stack(2, &dbt_dp, &dbr_dp);
-		return Matrix_(3,3,
-				-ct, -st,  transformed_y,
-				 st, -ct, -transformed_x,
-				0.0, 0.0, -1.0
-				);
 	}
 
 	Matrix Dbetween2(const Pose2& p1, const Pose2& p2) {
-		double ct=cos(p1.theta()), st=sin(p1.theta());
+	  Matrix db_dt2 = p1.r().transpose();
 	  return Matrix_(3,3,
-				 ct,  st, 0.0,
+	      db_dt2.data()[0], db_dt2.data()[1], 0.0,
-				-st,  ct, 0.0,
+	      db_dt2.data()[2], db_dt2.data()[3], 0.0,
-				0.0, 0.0, 1.0
+	      0.0,              0.0,              1.0);
-				);
 	}
 
 	/* ************************************************************************* */
-
 } // namespace gtsam

cpp/Pose2.h

@@ -1,7 +1,8 @@
 /**
  * @file  Pose2.h
- * @brief 3D Pose
+ * @brief 2D Pose
  * @author: Frank Dellaert
+ * @author: Richard Roberts
  */
 
 // \callgraph
@@ -9,45 +10,49 @@
 #pragma once
 
 #include "Point2.h"
+#include "Rot2.h"
 #include "Matrix.h"
 #include "Testable.h"
 
 namespace gtsam {
 
-	/**
+/**
-	 * A 2D pose (x,y,theta)
+ * A 2D pose (Point2,Rot2)
  */
-	class Pose2: Testable<Pose2>  {
+class Pose2: Testable<Pose2>  {
 
-	private:
+private:
-		double x_, y_, theta_;
+  Point2 t_;
+  Rot2 r_;
 
-	public:
+public:
 
   /** default constructor = origin */
   Pose2() :
-			x_(0), y_(0), theta_(0) {
+    t_(0.0, 0.0), r_(0) { } // default is origin
-		} // default is origin
 
   /** copy constructor */
   Pose2(const Pose2& pose) :
-			x_(pose.x_), y_(pose.y_), theta_(pose.theta_) {
+    t_(pose.t_), r_(pose.r_) { }
-		}
 
   /**
    * construct from (x,y,theta)
-		 * @param x x oordinate
+   * @param x x coordinate
    * @param y y coordinate
    * @param theta angle with positive X-axis
    */
   Pose2(double x, double y, double theta) :
-			x_(x), y_(y), theta_(theta) {
+    t_(x,y), r_(theta) { }
-		}
 
   /** construct from rotation and translation */
   Pose2(const Point2& t, double theta) :
-			x_(t.x()), y_(t.y()), theta_(theta) {
+    t_(t), r_(theta) { }
-		}
+  Pose2(double theta, const Point2& t) :
+    t_(t), r_(theta) { }
+  Pose2(const Point2& t, const Rot2& r) :
+    t_(t), r_(r) { }
+  Pose2(const Rot2& r, const Point2& t) :
+    t_(t), r_(r) { }
 
   /** print with optional string */
   void print(const std::string& s = "") const;
@@ -56,46 +61,66 @@ namespace gtsam {
   bool equals(const Pose2& pose, double tol = 1e-9) const;
 
   /** get functions for x, y, theta */
-		inline double x()     const { return x_;}
+  double x()     const { return t_.x();}
-		inline double y()     const { return y_;}
+  double y()     const { return t_.y();}
-		inline double theta() const { return theta_;}
+  double theta() const { return r_.theta();}
+  Point2 t()     const { return t_; }
+  Rot2 r()       const { return r_; }
 
   /** return DOF, dimensionality of tangent space = 3 */
-		size_t dim() const {
+  size_t dim() const { return 3; }
-			return 3;
-		}
 
   /* exponential map */
   Pose2 exmap(const Vector& v) const;
 
-    /** return vectorized form (column-wise)*/
+  /** return vectorized form (column-wise) */
   Vector vector() const;
 
   /** rotate pose by theta */
-    Pose2 rotate(double theta) const;
+//  Pose2 rotate(double theta) const;
+
+  /** inverse transformation */
+  Pose2 inverse() const {
+    return Pose2(r_, r_.unrotate(t_));
+  }
+
+  /** compose this transformation onto another (pre-multiply this*p1) */
+  Pose2 compose(const Pose2& p1) const {
+    return Pose2(p1.r_*r_, p1.r_*t_+p1.t_);
+  }
+
+  /** same as compose (pre-multiply this*p1) */
+  Pose2 operator*(const Pose2& p1) const {
+    return compose(p1);
+  }
+
+  /** Return point coordinates in pose coordinate frame, same as transform_to */
+  Point2 operator*(const Point2& point) const {
+    return r_.unrotate(point-t_);
+  }
 
   // operators
   Pose2 operator+(const Pose2& p2) const {
-  		return Pose2(x_ + p2.x_, y_ + p2.y_, theta_ + p2.theta_);
+    return Pose2(t_+p2.t_, r_*p2.r_);
   }
 
   Pose2 operator-(const Pose2& p2) const {
-  		return Pose2(x_ - p2.x_, y_ - p2.y_, theta_ - p2.theta_);
+    return Pose2(t_-p2.t_, r_.invcompose(p2.r_));
   }
-	}; // Pose2
+}; // Pose2
 
-	/**
+/**
  * Return point coordinates in pose coordinate frame
  */
-	Point2 transform_to(const Pose2& pose, const Point2& point);
+Point2 transform_to(const Pose2& pose, const Point2& point);
-	Matrix Dtransform_to1(const Pose2& pose, const Point2& point);
+Matrix Dtransform_to1(const Pose2& pose, const Point2& point);
-	Matrix Dtransform_to2(const Pose2& pose, const Point2& point);
+Matrix Dtransform_to2(const Pose2& pose, const Point2& point);
 
-	/**
+/**
  * Return relative pose between p1 and p2, in p1 coordinate frame
  */
-	Pose2 between(const Pose2& p1, const Pose2& p2);
+Pose2 between(const Pose2& p1, const Pose2& p2);
-	Matrix Dbetween1(const Pose2& p1, const Pose2& p2);
+Matrix Dbetween1(const Pose2& p1, const Pose2& p2);
-	Matrix Dbetween2(const Pose2& p1, const Pose2& p2);
+Matrix Dbetween2(const Pose2& p1, const Pose2& p2);
 
 } // namespace gtsam

cpp/Rot2.cpp

@@ -29,19 +29,29 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	Matrix Rot2::matrix() const {
-		double r[] = { c_, -s_, s_, c_ };
+		return Matrix_(2, 2, c_, -s_, s_, c_);
-		return Matrix_(2, 2, r);
 	}
 
 	/* ************************************************************************* */
 	Matrix Rot2::transpose() const {
-		double r[] = { c_, s_, -s_, c_ };
+		return Matrix_(2, 2, c_, s_, -s_, c_);
-		return Matrix_(2, 2, r);
+	}
+
+    /* ************************************************************************* */
+    Matrix Rot2::negtranspose() const {
+        return Matrix_(2, 2, -c_, -s_, s_, -c_);
     }
 
 	/* ************************************************************************* */
 	Rot2 Rot2::inverse() const { return Rot2(c_, -s_);}
 
+    /* ************************************************************************* */
+    Rot2 Rot2::invcompose(const Rot2& R) const {
+      return Rot2(
+           c_ * R.c_ + s_ * R.s_,
+          -s_ * R.c_ + c_ * R.s_);
+    }
+
 	/* ************************************************************************* */
 	Rot2 Rot2::operator*(const Rot2& R) const {
 		return Rot2(

cpp/Rot2.h

@@ -34,7 +34,14 @@ namespace gtsam {
 		Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}
 
 		/** return angle */
-		inline double angle() const {	return atan2(s_, c_);	}
+		double theta() const { return atan2(s_,c_); }
+		double angle() const { return atan2(s_,c_); }
+
+		/** return cos */
+		double c() const { return c_; }
+
+		/** return sin */
+		double s() const { return s_; }
 
 		/** print */
 		void print(const std::string& s = "theta") const;
@@ -54,12 +61,18 @@ namespace gtsam {
 		/** return 2*2 rotation matrix */
 		Matrix matrix() const;
 
-		/** return 3*3 transpose (inverse) rotation matrix   */
+		/** return 2*2 transpose (inverse) rotation matrix   */
 		Matrix transpose() const;
 
+		/** return 2*2 negative transpose */
+		Matrix negtranspose() const;
+
 		/** inverse transformation  */
 		Rot2 inverse() const;
 
+		/** compose with the inverse of this rotation */
+		Rot2 invcompose(const Rot2& R) const;
+
 		/** composition via sum and difference formulas */
 		Rot2 operator*(const Rot2& R) const;
 

cpp/testPose2.cpp

@@ -8,6 +8,7 @@
 #include "numericalDerivative.h"
 #include "Pose2.h"
 #include "Point2.h"
+#include "Rot2.h"
 
 using namespace gtsam;
 
@@ -20,13 +21,22 @@ TEST(Pose2, constructors) {
 }
 
 /* ************************************************************************* */
-TEST(Pose2, rotate) {
+TEST(Pose2, exmap) {
-	double theta = 0.1, c=cos(theta),s=sin(theta);
+  Pose2 pose(Point2(1,2), M_PI_2);
-	Pose2 p1(1,0,0.2), p2(0,1,0.4);
+  Pose2 expected(Point2(1.01, 1.985), M_PI_2+0.018);
-	CHECK(assert_equal(Pose2( c,s,0.3),p1.rotate(theta)));
+  Pose2 actual = pose.exmap(Vector_(3, 0.01, -0.015, 0.018));
-	CHECK(assert_equal(Pose2(-s,c,0.5),p2.rotate(theta)));
+  CHECK(assert_equal(expected, actual));
 }
 
+/* ************************************************************************* */
+//TEST(Pose2, rotate) {
+//  std::cout << "rotate\n";
+//	double theta = 0.1, c=cos(theta),s=sin(theta);
+//	Pose2 p1(1,0,0.2), p2(0,1,0.4);
+//	CHECK(assert_equal(Pose2( c,s,0.3),p1.rotate(theta)));
+//	CHECK(assert_equal(Pose2(-s,c,0.5),p2.rotate(theta)));
+//}
+
 /* ************************************************************************* */
 TEST(Pose2, operators) {
 	CHECK(assert_equal(Pose2(2,2,2),Pose2(1,1,1)+Pose2(1,1,1)));
@@ -60,6 +70,37 @@ TEST( Pose2, transform_to )
 	CHECK(assert_equal(numericalH2,actualH2));
 }
 
+/* ************************************************************************* */
+TEST(Pose2, compose_a)
+{
+  Pose2 pose1(Point2(.75, .5), Rot2(M_PI/10.0));
+  Pose2 pose2(Point2(0.701289620636, 1.34933052585), Rot2(M_PI/4.0-M_PI/10.0));
+
+  Pose2 pose_expected(Point2(1.0, 2.0), Rot2(M_PI/4.0));
+
+  Pose2 pose_actual_op = pose2 * pose1;
+  Pose2 pose_actual_fcn = pose2.compose(pose1);
+
+  CHECK(assert_equal(pose_expected, pose_actual_op));
+  CHECK(assert_equal(pose_expected, pose_actual_fcn));
+}
+
+/* ************************************************************************* */
+TEST(Pose2, compose_b)
+{
+  Pose2 pose1(Point2(1.0, 1.0), Rot2(M_PI/4.0));
+  Pose2 pose2(Point2(sqrt(.5), sqrt(.5)), Rot2(M_PI/4.0));
+
+  Pose2 pose_expected(Point2(1.0, 2.0), Rot2(M_PI/2.0));
+
+  Pose2 pose_actual_op = pose2 * pose1;
+  Pose2 pose_actual_fcn = pose2.compose(pose1);
+
+  CHECK(assert_equal(pose_expected, pose_actual_op));
+  CHECK(assert_equal(pose_expected, pose_actual_fcn));
+}
+
+
 /* ************************************************************************* */
 TEST( Pose2, between )
 {