From 4f59f58cd1bd07a567f0455c34fe8dd42f578ace Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sun, 8 Jan 2012 20:43:17 +0000 Subject: [PATCH] retract and localCoordinates optional argument in Pose3 to switch between different math versions, and unit testing all versions --- gtsam/geometry/Pose3.cpp | 72 ++++++++++++++++-------------- gtsam/geometry/Pose3.h | 16 ++++++- gtsam/geometry/tests/testPose3.cpp | 68 +++++++++++++++++++--------- 3 files changed, 99 insertions(+), 57 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 0f1f5e295..253c1cc84 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -97,52 +97,58 @@ namespace gtsam { /* ************************************************************************* */ // Different versions of retract - Pose3 Pose3::retract(const Vector& xi) const { -#ifdef CORRECT_POSE3_EXMAP - // Lie group exponential map, traces out geodesic - return compose(Expmap(xi)); -#else - Vector omega(sub(xi, 0, 3)); - Point3 v(sub(xi, 3, 6)); + Pose3 Pose3::retract(const Vector& xi, Pose3::CoordinatesMode mode) const { + if(mode == Pose3::FIRST_ORDER) { + Vector omega(sub(xi, 0, 3)); + Point3 v(sub(xi, 3, 6)); - // R is always done exactly in all three retract versions below - Rot3 R = R_.retract(omega); + // R is always done exactly in all three retract versions below + Rot3 R = R_.retract(omega); - // Incorrect version - // Retracts R and t independently - // Point3 t = t_.retract(v.vector()); + // Incorrect version + // Retracts R and t independently + // Point3 t = t_.retract(v.vector()); - // First order t approximation - Point3 t = t_ + R_ * v; + // First order t approximation + Point3 t = t_ + R_ * v; - // Second order t approximation - // Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2); + // Second order t approximation + // Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2); - return Pose3(R, t); -#endif + return Pose3(R, t); + } else if(mode == Pose3::CORRECT_EXPMAP) { + // Lie group exponential map, traces out geodesic + return compose(Expmap(xi)); + } else { + assert(false); + exit(1); + } } /* ************************************************************************* */ // different versions of localCoordinates - Vector Pose3::localCoordinates(const Pose3& T) const { -#ifdef CORRECT_POSE3_EXMAP - // Lie group logarithm map, exact inverse of exponential map - return Logmap(between(T)); -#else - // R is always done exactly in all three retract versions below - Vector omega = R_.localCoordinates(T.rotation()); + Vector Pose3::localCoordinates(const Pose3& T, Pose3::CoordinatesMode mode) const { + if(mode == Pose3::FIRST_ORDER) { + // R is always done exactly in all three retract versions below + Vector omega = R_.localCoordinates(T.rotation()); - // Incorrect version - // Independently computes the logmap of the translation and rotation - // Vector v = t_.localCoordinates(T.translation()); + // Incorrect version + // Independently computes the logmap of the translation and rotation + // Vector v = t_.localCoordinates(T.translation()); - // Correct first order t inverse - Point3 d = R_.unrotate(T.translation() - t_); + // Correct first order t inverse + Point3 d = R_.unrotate(T.translation() - t_); - // TODO: correct second order t inverse + // TODO: correct second order t inverse - return Vector_(6,omega(0),omega(1),omega(2),d.x(),d.y(),d.z()); -#endif + return Vector_(6,omega(0),omega(1),omega(2),d.x(),d.y(),d.z()); + } else if(mode == Pose3::CORRECT_EXPMAP) { + // Lie group logarithm map, exact inverse of exponential map + return Logmap(between(T)); + } else { + assert(false); + exit(1); + } } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 469ee71b5..1073d0773 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -18,6 +18,10 @@ #pragma once +#ifndef POSE3_DEFAULT_COORDINATES_MODE +#define POSE3_DEFAULT_COORDINATES_MODE Pose3::FIRST_ORDER +#endif + #include #include @@ -98,6 +102,14 @@ namespace gtsam { /// @name Manifold /// @{ + /** Enum to indicate which method should be used in Pose3::retract() and + * Pose3::localCoordinates() + */ + enum CoordinatesMode { + FIRST_ORDER, ///< A fast first-order approximation to the exponential map. + CORRECT_EXPMAP ///< The correct exponential map, computationally expensive. + }; + /// Dimensionality of tangent space = 6 DOF - used to autodetect sizes static size_t Dim() { return dimension; } @@ -106,10 +118,10 @@ namespace gtsam { /// Retraction from R^6 to Pose3 manifold neighborhood around current pose - Pose3 retract(const Vector& d) const; + Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode = POSE3_DEFAULT_COORDINATES_MODE) const; /// Local 6D coordinates of Pose3 manifold neighborhood around current pose - Vector localCoordinates(const Pose3& T2) const; + Vector localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode = POSE3_DEFAULT_COORDINATES_MODE) const; /// @} /// @name Lie Group diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 4ec0856bd..6c6289844 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -44,18 +44,25 @@ TEST( Pose3, equals) } /* ************************************************************************* */ -TEST( Pose3, retract) +TEST( Pose3, retract_first_order) { Pose3 id; Vector v = zero(6); v(0) = 0.3; - EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v),1e-2)); -#ifdef CORRECT_POSE3_EXMAP - v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; -#else + EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v, Pose3::FIRST_ORDER),1e-2)); v(3)=0.2;v(4)=0.7;v(5)=-2; -#endif - EXPECT(assert_equal(Pose3(R, P),id.retract(v),1e-2)); + EXPECT(assert_equal(Pose3(R, P),id.retract(v, Pose3::FIRST_ORDER),1e-2)); +} + +/* ************************************************************************* */ +TEST( Pose3, retract_expmap) +{ + Pose3 id; + Vector v = zero(6); + v(0) = 0.3; + EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v, Pose3::CORRECT_EXPMAP),1e-2)); + v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; + EXPECT(assert_equal(Pose3(R, P),id.retract(v, Pose3::CORRECT_EXPMAP),1e-2)); } /* ************************************************************************* */ @@ -443,31 +450,48 @@ TEST( Pose3, transformPose_to) } /* ************************************************************************* */ -TEST(Pose3, localCoordinates) +TEST(Pose3, localCoordinates_first_order) { Vector d12 = repeat(6,0.1); - Pose3 t1 = T, t2 = t1.retract(d12); - EXPECT(assert_equal(d12, t1.localCoordinates(t2))); + Pose3 t1 = T, t2 = t1.retract(d12, Pose3::FIRST_ORDER); + EXPECT(assert_equal(d12, t1.localCoordinates(t2, Pose3::FIRST_ORDER))); } /* ************************************************************************* */ -TEST(Pose3, manifold) +TEST(Pose3, localCoordinates_expmap) +{ + Vector d12 = repeat(6,0.1); + Pose3 t1 = T, t2 = t1.retract(d12, Pose3::CORRECT_EXPMAP); + EXPECT(assert_equal(d12, t1.localCoordinates(t2, Pose3::CORRECT_EXPMAP))); +} + +/* ************************************************************************* */ +TEST(Pose3, manifold_first_order) { Pose3 t1 = T; Pose3 t2 = T3; Pose3 origin; - Vector d12 = t1.localCoordinates(t2); - EXPECT(assert_equal(t2, t1.retract(d12))); - // todo: richard - commented out because this tests for "compose-style" (new) expmap - // EXPECT(assert_equal(t2, retract(origin,d12)*t1)); - Vector d21 = t2.localCoordinates(t1); - EXPECT(assert_equal(t1, t2.retract(d21))); - // todo: richard - commented out because this tests for "compose-style" (new) expmap - // EXPECT(assert_equal(t1, retract(origin,d21)*t2)); - - // Check that log(t1,t2)=-log(t2,t1) TODO: only holds for exp map - // EXPECT(assert_equal(d12,-d21)); + Vector d12 = t1.localCoordinates(t2, Pose3::FIRST_ORDER); + EXPECT(assert_equal(t2, t1.retract(d12, Pose3::FIRST_ORDER))); + Vector d21 = t2.localCoordinates(t1, Pose3::FIRST_ORDER); + EXPECT(assert_equal(t1, t2.retract(d21, Pose3::FIRST_ORDER))); } + +/* ************************************************************************* */ +TEST(Pose3, manifold_expmap) +{ + Pose3 t1 = T; + Pose3 t2 = T3; + Pose3 origin; + Vector d12 = t1.localCoordinates(t2, Pose3::CORRECT_EXPMAP); + EXPECT(assert_equal(t2, t1.retract(d12, Pose3::CORRECT_EXPMAP))); + Vector d21 = t2.localCoordinates(t1, Pose3::CORRECT_EXPMAP); + EXPECT(assert_equal(t1, t2.retract(d21, Pose3::CORRECT_EXPMAP))); + + // Check that log(t1,t2)=-log(t2,t1) + EXPECT(assert_equal(d12,-d21)); +} + /* ************************************************************************* */ TEST(Pose3, subgroups) {