From 8398c8a53d30bd0b2af6b39b2815bc32f2f2db4c Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 2 Feb 2011 05:17:21 +0000 Subject: [PATCH] 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 | 22 ++++-- gtsam/geometry/Pose2.h | 8 +++ gtsam/geometry/Pose3.cpp | 31 ++++++--- gtsam/geometry/Pose3.h | 8 +++ gtsam/geometry/tests/testPose2.cpp | 46 ++++++++++--- gtsam/geometry/tests/testPose3.cpp | 105 +++++++++++++++++++++++++++-- 6 files changed, 192 insertions(+), 28 deletions(-) diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index e42efb4ad..b403d7e57 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -47,10 +47,7 @@ namespace gtsam { } /* ************************************************************************* */ - -#ifdef SLOW_BUT_CORRECT_EXPMAP - - Pose2 Pose2::Expmap(const Vector& xi) { + Pose2 Pose2::ExpmapFull(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 /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 7eda0df8c..f594fb5ac 100644 --- a/gtsam/geometry/Pose2.h +++ b/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 */ diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 6fb1c567c..a6abef1f8 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/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) { - return compose(T,Pose3::Expmap(d)); +#ifdef CORRECT_POSE3_EXMAP + /* ************************************************************************* */ + // 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 diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 271589586..637af42eb 100644 --- a/gtsam/geometry/Pose3.h +++ b/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 diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 1283859ee..ed3628343 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/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(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))); diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 94da4ba0e..7ed84f25d 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/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(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(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(xi, range(0,3)); + Vector v = vector_range(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(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(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(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), Point3(2,1,0)), x2.expmap(x_step), tol)); - // FAILS: moves in global X, not inertial X -// EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI_4,0,0), Point3(3,2,0)), x3.expmap(sqrt(2) * 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.expmapFull(x_step), tol)); + EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI_4,0,0), Point3(2,2,0)), x3.expmapFull(sqrt(2) * x_step), tol)); } /* ************************************************************************* */