From af9320e468e36e5acec9b53bb11fcf29c726f0f9 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 10 Nov 2011 15:19:04 +0000 Subject: [PATCH] Cleaned up commented code --- gtsam/geometry/Pose3.cpp | 40 +++++++----------------------- gtsam/geometry/tests/testPose2.cpp | 4 +-- gtsam/geometry/tests/testPose3.cpp | 34 ++++++++++++------------- 3 files changed, 28 insertions(+), 50 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index b7eef0a2f..a7e687af1 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -95,46 +95,23 @@ namespace gtsam { } #ifdef CORRECT_POSE3_EXMAP -// /* ************************************************************************* */ -// // Changes default to use the full verions of expmap/logmap -// /* ************************************************************************* */ -// Pose3 Retract(const Vector& xi) { -// return Pose3::Expmap(xi); -// } -// -// /* ************************************************************************* */ -// Vector Unretract(const Pose3& p) { -// return Pose3::Logmap(p); -// } + /* ************************************************************************* */ + // Changes default to use the full verions of expmap/logmap + /* ************************************************************************* */ /* ************************************************************************* */ - Pose3 retract(const Vector& d) { - return retract(d); + Pose3 Pose3::retract(const Vector& d) { + return compose(Expmap(d)); } /* ************************************************************************* */ - Vector localCoordinates(const Pose3& T1, const Pose3& T2) { - return localCoordinates(T2); + Vector Pose3::localCoordinates(const Pose3& T1, const Pose3& T2) { + return Logmap(T1.between(T2)); } #else -// /* ************************************************************************* */ -// /* incorrect versions for which we know how to compute derivatives */ -// Pose3 Pose3::Retract(const Vector& d) { -// Vector w = sub(d, 0,3); -// Vector u = sub(d, 3,6); -// return Pose3(Rot3::Retract(w), Point3::Retract(u)); -// } -// -// /* ************************************************************************* */ -// // Log map at identity - return the translation and canonical rotation -// // coordinates of a pose. -// Vector Pose3::Unretract(const Pose3& p) { -// const Vector w = Rot3::Unretract(p.rotation()), u = Point3::Unretract(p.translation()); -// return concatVectors(2, &w, &u); -// } - + /* ************************************************************************* */ /** These are the "old-style" expmap and logmap about the specified * pose. Increments the offset and rotation independently given a translation and * canonical rotation coordinates. Created to match ML derivatives, but @@ -144,6 +121,7 @@ namespace gtsam { t_.retract(sub(d, 3, 6))); } + /* ************************************************************************* */ /** Independently computes the logmap of the translation and rotation. */ Vector Pose3::localCoordinates(const Pose3& pp) const { const Vector r(R_.localCoordinates(pp.rotation())), diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 0763cfba5..14e529613 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -150,8 +150,8 @@ namespace screw { TEST(Pose3, expmap_c) { EXPECT(assert_equal(screw::expected, expm(screw::xi),1e-6)); - EXPECT(assert_equal(screw::expected, Pose2::Retract(screw::xi),1e-6)); - EXPECT(assert_equal(screw::xi, Pose2::Unretract(screw::expected),1e-6)); + EXPECT(assert_equal(screw::expected, Pose2::Expmap(screw::xi),1e-6)); + EXPECT(assert_equal(screw::xi, Pose2::Logmap(screw::expected),1e-6)); } #endif diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index fd95f6973..cb336bb7e 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -105,24 +105,24 @@ namespace screw { TEST(Pose3, expmap_c) { EXPECT(assert_equal(screw::expected, expm(screw::xi),1e-6)); - EXPECT(assert_equal(screw::expected, Pose3::Retract(screw::xi),1e-6)); + EXPECT(assert_equal(screw::expected, Pose3::Expmap(screw::xi),1e-6)); } /* ************************************************************************* */ // assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi)) TEST(Pose3, Adjoint) { - Pose3 expected = T * Pose3::Retract(screw::xi) * inverse(T); + Pose3 expected = T * Pose3::Expmap(screw::xi) * inverse(T); Vector xiprime = Adjoint(T, screw::xi); - EXPECT(assert_equal(expected, Pose3::Retract(xiprime), 1e-6)); + EXPECT(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6)); - Pose3 expected2 = T2 * Pose3::Retract(screw::xi) * inverse(T2); + Pose3 expected2 = T2 * Pose3::Expmap(screw::xi) * inverse(T2); Vector xiprime2 = Adjoint(T2, screw::xi); - EXPECT(assert_equal(expected2, Pose3::Retract(xiprime2), 1e-6)); + EXPECT(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6)); - Pose3 expected3 = T3 * Pose3::Retract(screw::xi) * inverse(T3); + Pose3 expected3 = T3 * Pose3::Expmap(screw::xi) * inverse(T3); Vector xiprime3 = Adjoint(T3, screw::xi); - EXPECT(assert_equal(expected3, Pose3::Retract(xiprime3), 1e-6)); + EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6)); } /* ************************************************************************* */ @@ -145,7 +145,7 @@ TEST(Pose3, expmaps_galore) { Vector xi; Pose3 actual; xi = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6); - actual = Pose3::Retract(xi); + actual = Pose3::Expmap(xi); EXPECT(assert_equal(expm(xi), actual,1e-6)); EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6)); EXPECT(assert_equal(xi, localCoordinates(actual),1e-6)); @@ -153,17 +153,17 @@ TEST(Pose3, expmaps_galore) 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::Retract(txi); + actual = Pose3::Expmap(txi); EXPECT(assert_equal(expm(txi,30), actual,1e-6)); EXPECT(assert_equal(Agrawal06iros(txi), actual,1e-6)); Vector log = localCoordinates(actual); - EXPECT(assert_equal(actual, Pose3::Retract(log),1e-6)); + EXPECT(assert_equal(actual, Pose3::Expmap(log),1e-6)); EXPECT(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::Retract(xi); + actual = Pose3::Expmap(xi); EXPECT(assert_equal(expm(xi,10), actual,1e-5)); EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6)); EXPECT(assert_equal(xi, localCoordinates(actual),1e-6)); @@ -176,9 +176,9 @@ TEST(Pose3, Adjoint_compose) // 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::Retract(x) * T2; + Pose3 expected = T1 * Pose3::Expmap(x) * T2; Vector y = Adjoint(inverse(T2), x); - Pose3 actual = T1 * T2 * Pose3::Retract(y); + Pose3 actual = T1 * T2 * Pose3::Expmap(y); EXPECT(assert_equal(expected, actual, 1e-6)); } #endif // SLOW_BUT_CORRECT_EXMAP @@ -544,11 +544,11 @@ 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::Retract(-d),inverse(Pose3::Retract(d)))); + EXPECT(assert_equal(Pose3::Expmap(-d),inverse(Pose3::Expmap(d)))); // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) - Pose3 T2 = Pose3::Retract(2*d); - Pose3 T3 = Pose3::Retract(3*d); - Pose3 T5 = Pose3::Retract(5*d); + Pose3 T2 = Pose3::Expmap(2*d); + Pose3 T3 = Pose3::Expmap(3*d); + Pose3 T5 = Pose3::Expmap(5*d); EXPECT(assert_equal(T5,T2*T3)); EXPECT(assert_equal(T5,T3*T2));