Added Pose2::matrix() function.

cpp/Pose2.cpp

@@ -13,6 +13,15 @@ namespace gtsam {
   /** Explicit instantiation of base class to export members */
   INSTANTIATE_LIE(Pose2);
 
+  /* ************************************************************************* */
+  Matrix Pose2::matrix() const {
+  	Matrix R = r_.matrix();
+  	Matrix Z = zeros(1,2);
+  	R = stack(2, &R, &Z);
+  	Matrix T = Matrix_(3,1, t_.x(), t_.y(), 1.0);
+  	return collect(2, &R, &T);
+  }
+
   /* ************************************************************************* */
   void Pose2::print(const string& s) const {
     cout << s << "(" << t_.x() << ", " << t_.y() << ", " << r_.theta() << ")" << endl;

cpp/Pose2.h

@@ -52,6 +52,9 @@ namespace gtsam {
     /** assert equality up to a tolerance */
     bool equals(const Pose2& pose, double tol = 1e-9) const;
 
+    /** return transformation matrix */
+    Matrix matrix() const;
+
     /** get functions for x, y, theta */
     double x()     const { return t_.x(); }
     double y()     const { return t_.y(); }