From 53946b28d0c5969ac6a2f82ed824a811ab6902f4 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 4 Jan 2012 14:23:50 +0000
Subject: [PATCH] Fixed calls of adjointMap and tested correct exponential map
 derivatives based on them

---
 gtsam/geometry/Pose3.cpp           | 13 ++++++-------
 gtsam/geometry/tests/testPose3.cpp | 20 ++++++++++++++++++--
 2 files changed, 24 insertions(+), 9 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index 6c6860f0f..528400d6f 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -31,7 +31,7 @@ namespace gtsam {
 
   static const Matrix I3 = eye(3), Z3 = zeros(3, 3);
 #ifdef CORRECT_POSE3_EXMAP
-  static const _I3=-I3, I6 = eye(6);
+  static const Matrix _I3=-I3, I6 = eye(6);
 #endif
 
   /* ************************************************************************* */
@@ -100,13 +100,13 @@ namespace gtsam {
   /* ************************************************************************* */
 
   /* ************************************************************************* */
-  Pose3 Pose3::retract(const Vector& d) {
+  Pose3 Pose3::retract(const Vector& d) const {
   	return compose(Expmap(d));
   }
 
   /* ************************************************************************* */
-  Vector Pose3::localCoordinates(const Pose3& T1, const Pose3& T2) {
-  	return Logmap(T1.between(T2));
+  Vector Pose3::localCoordinates(const Pose3& T2) const {
+  	return Logmap(between(T2));
   }
 
 #else
@@ -188,7 +188,7 @@ namespace gtsam {
 		  	boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
 	  if (H1) {
 #ifdef CORRECT_POSE3_EXMAP
-		*H1 = adjointMap(inverse(p2)); // FIXME: this function doesn't exist with this interface
+		*H1 = p2.inverse().adjointMap();
 #else
 		const Rot3& R2 = p2.rotation();
 		const Point3& t2 = p2.translation();
@@ -216,8 +216,7 @@ namespace gtsam {
   Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const {
   	if (H1)
 #ifdef CORRECT_POSE3_EXMAP
-  		// FIXME: this function doesn't exist with this interface - should this be "*H1 = -adjointMap();" ?
-  	{ *H1 = - adjointMap(p); }
+  	{ *H1 = - adjointMap(); }
 #else
   	{
   		Matrix Rt = R_.transpose();
diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
index 94b81cb0c..df491b91f 100644
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@@ -100,6 +100,7 @@ namespace screw {
 }
 
 /* ************************************************************************* */
+// Checks correct exponential map (Expmap) with brute force matrix exponential
 TEST(Pose3, expmap_c_full)
 {
   EXPECT(assert_equal(screw::expected, expm<Pose3>(screw::xi),1e-6));
@@ -124,7 +125,7 @@ TEST(Pose3, Adjoint_full)
 }
 
 /* ************************************************************************* */
-/** Agrawal06iros version */
+/** Agrawal06iros version of exponential map */
 Pose3 Agrawal06iros(const Vector& xi) {
 	Vector w = xi.head(3);
 	Vector v = xi.tail(3);
@@ -181,6 +182,8 @@ TEST(Pose3, Adjoint_compose_full)
 }
 
 /* ************************************************************************* */
+// Check compose and its pushforward
+// NOTE: testing::compose<Pose3>(t1,t2) = t1.compose(t2)  (see lieProxies.h)
 TEST( Pose3, compose )
 {
 	Matrix actual = (T2*T2).matrix();
@@ -192,12 +195,16 @@ TEST( Pose3, compose )
 
 	Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T2, T2);
 	EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3));
+#ifdef CORRECT_POSE3_EXMAP
+	EXPECT(assert_equal(T2.inverse().adjointMap(),actualDcompose1,5e-3));
+#endif
 
 	Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T2, T2);
 	EXPECT(assert_equal(numericalH2,actualDcompose2,1e-4));
 }
 
 /* ************************************************************************* */
+// Check compose and its pushforward, another case
 TEST( Pose3, compose2 )
 {
 	const Pose3& T1 = T;
@@ -210,6 +217,9 @@ TEST( Pose3, compose2 )
 
 	Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T1, T2);
 	EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3));
+#ifdef CORRECT_POSE3_EXMAP
+	EXPECT(assert_equal(T2.inverse().adjointMap(),actualDcompose1,5e-3));
+#endif
 
 	Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T1, T2);
 	EXPECT(assert_equal(numericalH2,actualDcompose2,1e-5));
@@ -225,6 +235,9 @@ TEST( Pose3, inverse)
 
 	Matrix numericalH = numericalDerivative11(testing::inverse<Pose3>, T);
 	EXPECT(assert_equal(numericalH,actualDinverse,5e-3));
+#ifdef CORRECT_POSE3_EXMAP
+	EXPECT(assert_equal(-T.adjointMap(),actualDinverse,5e-3));
+#endif
 }
 
 /* ************************************************************************* */
@@ -238,6 +251,9 @@ TEST( Pose3, inverseDerivatives2)
 	Matrix actualDinverse;
 	T.inverse(actualDinverse);
 	EXPECT(assert_equal(numericalH,actualDinverse,5e-3));
+#ifdef CORRECT_POSE3_EXMAP
+	EXPECT(assert_equal(-T.adjointMap(),actualDinverse,5e-3));
+#endif
 }
 
 /* ************************************************************************* */
@@ -460,7 +476,7 @@ TEST(Pose3, manifold)
 	// lines in canonical coordinates correspond to Abelian subgroups in SE(3)
 	 Vector d = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6);
 	// exp(-d)=inverse(exp(d))
-	 EXPECT(assert_equal(Pose3::Expmap(-d),inverse(Pose3::Expmap(d))));
+	 EXPECT(assert_equal(Pose3::Expmap(-d),Pose3::Expmap(d).inverse()));
 	// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
 	 Pose3 T2 = Pose3::Expmap(2*d);
 	 Pose3 T3 = Pose3::Expmap(3*d);