diff --git a/cpp/Pose2.cpp b/cpp/Pose2.cpp
index 1ac3008fa..7fc5ffc2b 100644
--- a/cpp/Pose2.cpp
+++ b/cpp/Pose2.cpp
@@ -60,6 +60,7 @@ namespace gtsam {
 	}
 
   /* ************************************************************************* */
+  // see doc/math.lyx, SE(2) section
   Point2 transform_to(const Pose2& pose, const Point2& point, boost::optional<
 			Matrix&> H1, boost::optional<Matrix&> H2) {
 		const Rot2& R = pose.r();
@@ -73,13 +74,8 @@ namespace gtsam {
 		return q;
 	}
 
-  Matrix Dtransform_to1(const Pose2& pose, const Point2& point) {
-		Matrix H; transform_to(pose, point, H, boost::none); return H;
-  }
-
-  Matrix Dtransform_to2(const Pose2& pose, const Point2& point) {
-		Matrix H; transform_to(pose, point, boost::none, H); return H;
-  }
+  /* ************************************************************************* */
+  // see doc/math.lyx, SE(2) section
 
   Matrix Dcompose1(const Pose2& p1, const Pose2& p2) {
 		return AdjointMap(inverse(p2));
@@ -145,11 +141,11 @@ namespace gtsam {
 	Rot2 bearing(const Pose2& pose, const Point2& point,
 			boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
 		if (!H1 && !H2) return bearing(pose, point);
-		Point2 d = transform_to(pose, point);
+		Point2 d = transform_to(pose, point, H1, H2);
 		Matrix D_result_d;
 		Rot2 result = relativeBearing(d, D_result_d);
-		if (H1) *H1 = D_result_d * Dtransform_to1(pose, point);
-		if (H2) *H2 = D_result_d * Dtransform_to2(pose, point);
+		if (H1) *H1 = D_result_d * (*H1);
+		if (H2) *H2 = D_result_d * (*H2);
 		return result;
 	}
 
@@ -162,11 +158,11 @@ namespace gtsam {
 	double range(const Pose2& pose, const Point2& point,
 			boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
 		if (!H1 && !H2) return range(pose, point);
-		Point2 d = transform_to(pose, point);
+		Point2 d = transform_to(pose, point, H1, H2);
 		double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
 		Matrix D_result_d = Matrix_(1, 2, x / n, y / n);
-		if (H1) *H1 = D_result_d * Dtransform_to1(pose, point);
-		if (H2) *H2 = D_result_d * Dtransform_to2(pose, point);
+		if (H1) *H1 = D_result_d * (*H1);
+		if (H2) *H2 = D_result_d * (*H2);
 		return n;
 	}
 
diff --git a/cpp/Pose2.h b/cpp/Pose2.h
index 4b363c0f9..63fb5d223 100644
--- a/cpp/Pose2.h
+++ b/cpp/Pose2.h
@@ -100,19 +100,21 @@ namespace gtsam {
 
   /** Return point coordinates in pose coordinate frame */
   inline Point2 transform_to(const Pose2& pose, const Point2& point) {
-    return unrotate(pose.r(), point-pose.t()); }
+		return unrotate(pose.r(), point - pose.t());
+	}
   Point2 transform_to(const Pose2& pose, const Point2& point,
-			boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
-  Matrix Dtransform_to1(const Pose2& pose, const Point2& point);
-  Matrix Dtransform_to2(const Pose2& pose, const Point2& point);
+		boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
 
   /** Return point coordinates in global frame */
+  inline Point2 transform_from(const Pose2& pose, const Point2& point) {
+		return rotate(pose.r(), point) + pose.t();
+	}
   Point2 transform_from(const Pose2& pose, const Point2& point,
-  		boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none);
+  	boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
 
   /** Return relative pose between p1 and p2, in p1 coordinate frame */
   Pose2 between(const Pose2& p1, const Pose2& p2,
-  		boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
+  	boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
 
   /** same as compose (pre-multiply this*p1) */
   inline Pose2 operator*(const Pose2& p1, const Pose2& p0) { return compose(p1, p0); }
diff --git a/cpp/testPose2.cpp b/cpp/testPose2.cpp
index 47739d53d..d574af26c 100644
--- a/cpp/testPose2.cpp
+++ b/cpp/testPose2.cpp
@@ -84,33 +84,26 @@ TEST(Pose2, logmap) {
 /* ************************************************************************* */
 TEST( Pose2, transform_to )
 {
-  //cout << "transform_to" << endl;
   Pose2 pose(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y
   Point2 point(-1,4);    // landmark at (-1,4)
 
   // expected
   Point2 expected(2,2);
-//  Matrix expectedH1 = Matrix_(2,3, 0.0, -1.0, 2.0,  1.0, 0.0, -2.0);
-//  Matrix expectedH2 = Matrix_(2,2, 0.0, 1.0,  -1.0, 0.0);
+  Matrix expectedH1 = Matrix_(2,3, -1.0, 0.0, 2.0,  0.0, -1.0, -2.0);
+  Matrix expectedH2 = Matrix_(2,2, 0.0, 1.0,  -1.0, 0.0);
 
   // actual
-  Point2 actual = transform_to(pose,point);
-  Matrix actualH1 = Dtransform_to1(pose,point);
-  Matrix actualH2 = Dtransform_to2(pose,point);
-
+  Matrix actualH1, actualH2;
+  Point2 actual = transform_to(pose,point, actualH1, actualH2);
   CHECK(assert_equal(expected,actual));
-//  CHECK(assert_equal(expectedH1,actualH1));
-//  CHECK(assert_equal(expectedH2,actualH2));
 
+  CHECK(assert_equal(expectedH1,actualH1));
   Matrix numericalH1 = numericalDerivative21(transform_to, pose, point, 1e-5);
   CHECK(assert_equal(numericalH1,actualH1));
 
+  CHECK(assert_equal(expectedH2,actualH2));
   Matrix numericalH2 = numericalDerivative22(transform_to, pose, point, 1e-5);
   CHECK(assert_equal(numericalH2,actualH2));
-
-  transform_to(pose,point,actualH1,actualH2);
-  CHECK(assert_equal(numericalH1,actualH1));
-  CHECK(assert_equal(numericalH2,actualH2));
 }
 
 /* ************************************************************************* */