From 1aed18717b6bbff1fece1decf14b7823146308db Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 12 Jan 2010 02:08:41 +0000
Subject: [PATCH] combined evaluation and derivatives

---
 cpp/Pose2.cpp     | 20 +++++++++++++++-----
 cpp/Pose2.h       |  3 +++
 cpp/testPose2.cpp | 13 ++++---------
 3 files changed, 22 insertions(+), 14 deletions(-)

diff --git a/cpp/Pose2.cpp b/cpp/Pose2.cpp
index d48074d21..856909f68 100644
--- a/cpp/Pose2.cpp
+++ b/cpp/Pose2.cpp
@@ -24,15 +24,25 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  // TODO, have a combined function that returns both function value and derivative
+  Point2 transform_to(const Pose2& pose, const Point2& point, boost::optional<
+			Matrix&> H1, boost::optional<Matrix&> H2) {
+		const Rot2& R = pose.r();
+		Point2 d = point - pose.t();
+		Point2 q = R.unrotate(d);
+		if (!H1 && !H2) return q;
+		if (H1) *H1 = Matrix_(2, 3,
+					-1.0, 0.0,  q.y(),
+					0.0, -1.0, -q.x());
+		if (H2) *H2 = R.transpose();
+		return q;
+	}
+
   Matrix Dtransform_to1(const Pose2& pose, const Point2& point) {
-    Matrix dx_dt = Matrix_(2,2, -1.0, 0.0, 0.0, -1.0);
-    Matrix dx_dr = Dunrotate1(pose.r(), point-pose.t());
-    return collect(2, &dx_dt, &dx_dr);
+		Matrix H; transform_to(pose, point, H, boost::none); return H;
   }
 
   Matrix Dtransform_to2(const Pose2& pose, const Point2& point) {
-    return pose.r().transpose();
+		Matrix H; transform_to(pose, point, boost::none, H); return H;
   }
 
   /* ************************************************************************* */
diff --git a/cpp/Pose2.h b/cpp/Pose2.h
index 9fff2e885..4c5f17ce2 100644
--- a/cpp/Pose2.h
+++ b/cpp/Pose2.h
@@ -9,6 +9,7 @@
 
 #pragma once
 
+#include <boost/optional.hpp>
 #include "Matrix.h"
 #include "Testable.h"
 #include "Lie.h"
@@ -87,6 +88,8 @@ 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()); }
+  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);
 
diff --git a/cpp/testPose2.cpp b/cpp/testPose2.cpp
index ec0a22f72..ce50ed36b 100644
--- a/cpp/testPose2.cpp
+++ b/cpp/testPose2.cpp
@@ -65,15 +65,6 @@ TEST(Pose2, logmap) {
   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, transform_to )
 {
@@ -100,6 +91,10 @@ TEST( Pose2, transform_to )
 
   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));
 }
 
 /* ************************************************************************* */