Made versions with [expmap|logmap]Full() for Pose2 and Pose3 to allow access to complete expmap functions, while also allowing for the concurrent existence of the approximate expmap for optimization speed

gtsam/geometry/Pose2.cpp

@@ -47,10 +47,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-
+	Pose2 Pose2::ExpmapFull(const Vector& xi) {
-#ifdef SLOW_BUT_CORRECT_EXPMAP
-
-	Pose2 Pose2::Expmap(const Vector& xi) {
 	  assert(xi.size() == 3);
 		Point2 v(xi(0),xi(1));
 		double w = xi(2);
@@ -64,7 +61,8 @@ namespace gtsam {
 		}
 	}
 
-  Vector Pose2::Logmap(const Pose2& p) {
+	/* ************************************************************************* */
+  Vector Pose2::LogmapFull(const Pose2& p) {
   	const Rot2& R = p.r();
   	const Point2& t = p.t();
 		double w = R.theta();
@@ -79,6 +77,20 @@ namespace gtsam {
 		}
   }
 
+  /* ************************************************************************* */
+#ifdef SLOW_BUT_CORRECT_EXPMAP
+  /* ************************************************************************* */
+  // Changes default to use the full verions of expmap/logmap
+  /* ************************************************************************* */
+	Pose2 Pose2::Expmap(const Vector& xi) {
+	  return ExpmapFull(xi);
+	}
+
+	/* ************************************************************************* */
+  Vector Pose2::Logmap(const Pose2& p) {
+  	return LogmapFull(p);
+  }
+
 #else
 
   /* ************************************************************************* */

gtsam/geometry/Pose2.h

@@ -113,10 +113,18 @@ namespace gtsam {
      */
     static Vector Logmap(const Pose2& p);
 
+    /** non-approximated versions of Expmap/Logmap */
+  	static Pose2 ExpmapFull(const Vector& xi);
+    static Vector LogmapFull(const Pose2& p);
+
     /** default implementations of binary functions */
     inline Pose2 expmap(const Vector& v) const { return gtsam::expmap_default(*this, v); }
     inline Vector logmap(const Pose2& p2) const { return gtsam::logmap_default(*this, p2);}
 
+    /** non-approximated versions of expmap/logmap */
+    inline Pose2 expmapFull(const Vector& v) const { return compose(ExpmapFull(v)); }
+    inline Vector logmapFull(const Pose2& p2) const { return LogmapFull(between(p2));}
+
     /**
      * Return relative pose between p1 and p2, in p1 coordinate frame
      */

gtsam/geometry/Pose3.cpp

@@ -53,11 +53,8 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-
-#ifdef CORRECT_POSE3_EXMAP
-
   /** Modified from Murray94book version (which assumes w and v normalized?) */
-  template<> Pose3 expmap(const Vector& xi) {
+  Pose3 Pose3::ExpmapFull(const Vector& xi) {
 
   	// get angular velocity omega and translational velocity v from twist xi
   	Point3 w(xi(0),xi(1),xi(2)), v(xi(3),xi(4),xi(5));
@@ -77,8 +74,9 @@ namespace gtsam {
 		}
   }
 
-  Vector logmap(const Pose3& p) {
+  /* ************************************************************************* */
-    Vector w = logmap(p.rotation()), T = p.translation().vector();
+  Vector Pose3::LogmapFull(const Pose3& p) {
+    Vector w = Rot3::Logmap(p.rotation()), T = p.translation().vector();
   	double t = norm_2(w);
 		if (t < 1e-5)
 	    return concatVectors(2, &w, &T);
@@ -90,12 +88,27 @@ namespace gtsam {
 		}
   }
 
-  Pose3 expmap(const Pose3& T, const Vector& d) {
+#ifdef CORRECT_POSE3_EXMAP
-    return compose(T,Pose3::Expmap(d));
+  /* ************************************************************************* */
+  // Changes default to use the full verions of expmap/logmap
+  /* ************************************************************************* */
+  Pose3 Expmap(const Vector& xi) {
+  	return Pose3::ExpmapFull(xi);
   }
 
+  /* ************************************************************************* */
+  Vector Logmap(const Pose3& p) {
+  	return Pose3::LogmapFull(p);
+  }
+
+  /* ************************************************************************* */
+  Pose3 expmap(const Vector& d) {
+  	return expmapFull(d);
+  }
+
+  /* ************************************************************************* */
   Vector logmap(const Pose3& T1, const Pose3& T2) {
-    return Pose3::logmap(between(T1,T2));
+  	return logmapFull(T2);
   }
 
 #else

gtsam/geometry/Pose3.h

@@ -126,6 +126,14 @@ namespace gtsam {
     /** Logarithm map around another pose T1 */
     Vector logmap(const Pose3& T2) const;
 
+    /** non-approximated versions of Expmap/Logmap */
+  	static Pose3 ExpmapFull(const Vector& xi);
+    static Vector LogmapFull(const Pose3& p);
+
+    /** non-approximated versions of expmap/logmap */
+    inline Pose3 expmapFull(const Vector& v) const { return compose(Pose3::ExpmapFull(v)); }
+    inline Vector logmapFull(const Pose3& p2) const { return Pose3::LogmapFull(between(p2));}
+
     /**
      * Return relative pose between p1 and p2, in p1 coordinate frame
      * as well as optionally the derivatives

gtsam/geometry/tests/testPose2.cpp

@@ -36,7 +36,6 @@ using namespace std;
 
 /* ************************************************************************* */
 TEST(Pose2, constructors) {
-  //cout << "constructors" << endl;
   Point2 p;
   Pose2 pose(0,p);
   Pose2 origin;
@@ -47,7 +46,6 @@ TEST(Pose2, constructors) {
 
 /* ************************************************************************* */
 TEST(Pose2, manifold) {
-  //cout << "manifold" << endl;
 	Pose2 t1(M_PI_2, Point2(1, 2));
 	Pose2 t2(M_PI_2+0.018, Point2(1.015, 2.01));
 	Pose2 origin;
@@ -61,7 +59,6 @@ TEST(Pose2, manifold) {
 
 /* ************************************************************************* */
 TEST(Pose2, expmap) {
-  //cout << "expmap" << endl;
   Pose2 pose(M_PI_2, Point2(1, 2));
 #ifdef SLOW_BUT_CORRECT_EXPMAP
   Pose2 expected(1.00811, 2.01528, 2.5608);
@@ -72,6 +69,14 @@ TEST(Pose2, expmap) {
   EXPECT(assert_equal(expected, actual, 1e-5));
 }
 
+/* ************************************************************************* */
+TEST(Pose2, expmap_full) {
+  Pose2 pose(M_PI_2, Point2(1, 2));
+  Pose2 expected(1.00811, 2.01528, 2.5608);
+  Pose2 actual = pose.expmapFull(Vector_(3, 0.01, -0.015, 0.99));
+  EXPECT(assert_equal(expected, actual, 1e-5));
+}
+
 /* ************************************************************************* */
 TEST(Pose2, expmap2) {
   // do an actual series exponential map
@@ -90,7 +95,6 @@ TEST(Pose2, expmap2) {
 
 /* ************************************************************************* */
 TEST(Pose2, expmap0) {
-  //cout << "expmap0" << endl;
   Pose2 pose(M_PI_2, Point2(1, 2));
 #ifdef SLOW_BUT_CORRECT_EXPMAP
   Pose2 expected(1.01491, 2.01013, 1.5888);
@@ -101,6 +105,14 @@ TEST(Pose2, expmap0) {
   EXPECT(assert_equal(expected, actual, 1e-5));
 }
 
+/* ************************************************************************* */
+TEST(Pose2, expmap0_full) {
+  Pose2 pose(M_PI_2, Point2(1, 2));
+  Pose2 expected(1.01491, 2.01013, 1.5888);
+  Pose2 actual = pose * Pose2::ExpmapFull(Vector_(3, 0.01, -0.015, 0.018));
+  EXPECT(assert_equal(expected, actual, 1e-5));
+}
+
 #ifdef SLOW_BUT_CORRECT_EXPMAP
 /* ************************************************************************* */
 // test case for screw motion in the plane
@@ -120,9 +132,21 @@ TEST(Pose3, expmap_c)
 }
 #endif
 
+/* ************************************************************************* */
+TEST(Pose3, expmap_c_full)
+{
+  double w=0.3;
+	Vector xi = Vector_(3, 0.0, w, w);
+	Rot2 expectedR = Rot2::fromAngle(w);
+	Point2 expectedT(-0.0446635, 0.29552);
+	Pose2 expected(expectedR, expectedT);
+  EXPECT(assert_equal(expected, expm<Pose2>(xi),1e-6));
+  EXPECT(assert_equal(expected, Pose2::ExpmapFull(xi),1e-6));
+  EXPECT(assert_equal(xi, Pose2::LogmapFull(expected),1e-6));
+}
+
 /* ************************************************************************* */
 TEST(Pose2, logmap) {
-  //cout << "logmap" << endl;
   Pose2 pose0(M_PI_2, Point2(1, 2));
   Pose2 pose(M_PI_2+0.018, Point2(1.015, 2.01));
 #ifdef SLOW_BUT_CORRECT_EXPMAP
@@ -134,6 +158,15 @@ TEST(Pose2, logmap) {
   EXPECT(assert_equal(expected, actual, 1e-5));
 }
 
+/* ************************************************************************* */
+TEST(Pose2, logmap_full) {
+  Pose2 pose0(M_PI_2, Point2(1, 2));
+  Pose2 pose(M_PI_2+0.018, Point2(1.015, 2.01));
+  Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018);
+  Vector actual = pose0.logmapFull(pose);
+  EXPECT(assert_equal(expected, actual, 1e-5));
+}
+
 /* ************************************************************************* */
 Point2 transform_to_proxy(const Pose2& pose, const Point2& point) {
 	return pose.transform_to(point);
@@ -193,7 +226,6 @@ TEST (Pose2, transform_from)
 /* ************************************************************************* */
 TEST(Pose2, compose_a)
 {
-  //cout << "compose_a" << endl;
   Pose2 pose1(M_PI/4.0, Point2(sqrt(0.5), sqrt(0.5)));
   Pose2 pose2(M_PI/2.0, Point2(0.0, 2.0));
 
@@ -228,7 +260,6 @@ TEST(Pose2, compose_a)
 /* ************************************************************************* */
 TEST(Pose2, compose_b)
 {
-  //cout << "compose_b" << endl;
   Pose2 pose1(Rot2::fromAngle(M_PI/10.0), Point2(.75, .5));
   Pose2 pose2(Rot2::fromAngle(M_PI/4.0-M_PI/10.0), Point2(0.701289620636, 1.34933052585));
 
@@ -250,7 +281,6 @@ TEST(Pose2, compose_b)
 /* ************************************************************************* */
 TEST(Pose2, compose_c)
 {
-  //cout << "compose_c" << endl;
   Pose2 pose1(Rot2::fromAngle(M_PI/4.0), Point2(1.0, 1.0));
   Pose2 pose2(Rot2::fromAngle(M_PI/4.0), Point2(sqrt(.5), sqrt(.5)));
 

gtsam/geometry/tests/testPose3.cpp

@@ -56,6 +56,18 @@ TEST( Pose3, expmap_a)
   CHECK(assert_equal(Pose3(R, P),id.expmap(v),1e-5));
 }
 
+/* ************************************************************************* */
+TEST( Pose3, expmap_a_full)
+{
+  Pose3 id;
+  Vector v(6);
+  fill(v.begin(), v.end(), 0);
+  v(0) = 0.3;
+  CHECK(assert_equal(id.expmap(v), Pose3(R, Point3())));
+  v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
+  CHECK(assert_equal(Pose3(R, P),id.expmapFull(v),1e-5));
+}
+
 /* ************************************************************************* */
 TEST(Pose3, expmap_b)
 {
@@ -65,8 +77,6 @@ TEST(Pose3, expmap_b)
   CHECK(assert_equal(expected, p2));
 }
 
-#ifdef CORRECT_POSE3_EXMAP
-
 /* ************************************************************************* */
 // test case for screw motion in the plane
 namespace screw {
@@ -77,6 +87,8 @@ namespace screw {
 	Pose3 expected(expectedR, expectedT);
 }
 
+#ifdef CORRECT_POSE3_EXMAP
+/* ************************************************************************* */
 TEST(Pose3, expmap_c)
 {
   CHECK(assert_equal(screw::expected, expm<Pose3>(screw::xi),1e-6));
@@ -159,6 +171,88 @@ TEST(Pose3, Adjoint_compose)
 }
 #endif // SLOW_BUT_CORRECT_EXMAP
 
+/* ************************************************************************* */
+TEST(Pose3, expmap_c_full)
+{
+  CHECK(assert_equal(screw::expected, expm<Pose3>(screw::xi),1e-6));
+  CHECK(assert_equal(screw::expected, Pose3::ExpmapFull(screw::xi),1e-6));
+}
+
+/* ************************************************************************* */
+// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi))
+TEST(Pose3, Adjoint_full)
+{
+	Pose3 expected = T * Pose3::ExpmapFull(screw::xi) * T.inverse();
+	Vector xiprime = T.Adjoint(screw::xi);
+	CHECK(assert_equal(expected, Pose3::ExpmapFull(xiprime), 1e-6));
+
+	Pose3 expected2 = T2 * Pose3::ExpmapFull(screw::xi) * T2.inverse();
+	Vector xiprime2 = T2.Adjoint(screw::xi);
+	CHECK(assert_equal(expected2, Pose3::ExpmapFull(xiprime2), 1e-6));
+
+	Pose3 expected3 = T3 * Pose3::ExpmapFull(screw::xi) * T3.inverse();
+	Vector xiprime3 = T3.Adjoint(screw::xi);
+	CHECK(assert_equal(expected3, Pose3::ExpmapFull(xiprime3), 1e-6));
+}
+
+/* ************************************************************************* */
+/** Agrawal06iros version */
+using namespace boost::numeric::ublas;
+Pose3 Agrawal06iros(const Vector& xi) {
+	Vector w = vector_range<const Vector>(xi, range(0,3));
+	Vector v = vector_range<const Vector>(xi, range(3,6));
+	double t = norm_2(w);
+	if (t < 1e-5)
+		return Pose3(Rot3(), Point3::Expmap(v));
+	else {
+		Matrix W = skewSymmetric(w/t);
+		Matrix A = eye(3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W);
+		return Pose3(Rot3::Expmap (w), Point3::Expmap(A * v));
+	}
+}
+
+/* ************************************************************************* */
+TEST(Pose3, expmaps_galore_full)
+{
+	Vector xi; Pose3 actual;
+	xi = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6);
+	actual = Pose3::ExpmapFull(xi);
+  CHECK(assert_equal(expm<Pose3>(xi), actual,1e-6));
+  CHECK(assert_equal(Agrawal06iros(xi), actual,1e-6));
+  CHECK(assert_equal(xi, Pose3::LogmapFull(actual),1e-6));
+
+	xi = Vector_(6,0.1,-0.2,0.3,-0.4,0.5,-0.6);
+	for (double theta=1.0;0.3*theta<=M_PI;theta*=2) {
+		Vector txi = xi*theta;
+		actual = Pose3::ExpmapFull(txi);
+		CHECK(assert_equal(expm<Pose3>(txi,30), actual,1e-6));
+		CHECK(assert_equal(Agrawal06iros(txi), actual,1e-6));
+		Vector log = Pose3::LogmapFull(actual);
+		CHECK(assert_equal(actual, Pose3::ExpmapFull(log),1e-6));
+		CHECK(assert_equal(txi,log,1e-6)); // not true once wraps
+	}
+
+  // Works with large v as well, but expm needs 10 iterations!
+	xi = Vector_(6,0.2,0.3,-0.8,100.0,120.0,-60.0);
+	actual = Pose3::ExpmapFull(xi);
+  CHECK(assert_equal(expm<Pose3>(xi,10), actual,1e-5));
+  CHECK(assert_equal(Agrawal06iros(xi), actual,1e-6));
+  CHECK(assert_equal(xi, Pose3::LogmapFull(actual),1e-6));
+}
+
+/* ************************************************************************* */
+TEST(Pose3, Adjoint_compose_full)
+{
+	// To debug derivatives of compose, assert that
+	// T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2
+	const Pose3& T1 = T;
+	Vector x = Vector_(6,0.1,0.1,0.1,0.4,0.2,0.8);
+	Pose3 expected = T1 * Pose3::ExpmapFull(x) * T2;
+	Vector y = T2.inverse().Adjoint(x);
+	Pose3 actual = T1 * T2 * Pose3::ExpmapFull(y);
+	CHECK(assert_equal(expected, actual, 1e-6));
+}
+
 /* ************************************************************************* */
 TEST( Pose3, compose )
 {
@@ -510,10 +604,9 @@ TEST( Pose3, unicycle )
 {
 	// velocity in X should be X in inertial frame, rather than global frame
 	Vector x_step = delta(6,3,1.0);
-	EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), l1), x1.expmap(x_step), tol));
+	EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), l1), x1.expmapFull(x_step), tol));
-	EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), Point3(2,1,0)), x2.expmap(x_step), tol));
+	EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), Point3(2,1,0)), x2.expmapFull(x_step), tol));
-	// FAILS: moves in global X, not inertial X
+	EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI_4,0,0), Point3(2,2,0)), x3.expmapFull(sqrt(2) * x_step), tol));
-//	EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI_4,0,0), Point3(3,2,0)), x3.expmap(sqrt(2) * x_step), tol));
 }
 
 /* ************************************************************************* */