diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 253c1cc84..29e335d02 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -98,7 +98,10 @@ namespace gtsam { /* ************************************************************************* */ // Different versions of retract Pose3 Pose3::retract(const Vector& xi, Pose3::CoordinatesMode mode) const { - if(mode == Pose3::FIRST_ORDER) { + if(mode == Pose3::EXPMAP) { + // Lie group exponential map, traces out geodesic + return compose(Expmap(xi)); + } else if(mode == Pose3::FIRST_ORDER) { Vector omega(sub(xi, 0, 3)); Point3 v(sub(xi, 3, 6)); @@ -116,9 +119,6 @@ namespace gtsam { // Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2); 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); @@ -128,7 +128,10 @@ namespace gtsam { /* ************************************************************************* */ // different versions of localCoordinates Vector Pose3::localCoordinates(const Pose3& T, Pose3::CoordinatesMode mode) const { - if(mode == Pose3::FIRST_ORDER) { + if(mode == Pose3::EXPMAP) { + // Lie group logarithm map, exact inverse of exponential map + return Logmap(between(T)); + } else if(mode == Pose3::FIRST_ORDER) { // R is always done exactly in all three retract versions below Vector omega = R_.localCoordinates(T.rotation()); @@ -142,9 +145,6 @@ namespace gtsam { // TODO: correct second order t inverse 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 1073d0773..bad34d8d5 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -106,8 +106,8 @@ namespace gtsam { * Pose3::localCoordinates() */ enum CoordinatesMode { - FIRST_ORDER, ///< A fast first-order approximation to the exponential map. - CORRECT_EXPMAP ///< The correct exponential map, computationally expensive. + EXPMAP, ///< The correct exponential map, computationally expensive. + FIRST_ORDER ///< A fast first-order approximation to the exponential map. }; /// Dimensionality of tangent space = 6 DOF - used to autodetect sizes diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 7d405a6e7..986aa168c 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -22,7 +22,7 @@ #pragma once #ifndef ROT3_DEFAULT_COORDINATES_MODE -#define ROT3_DEFAULT_COORDINATES_MODE Rot3::FIRST_ORDER +#define ROT3_DEFAULT_COORDINATES_MODE Rot3::CALEY #endif #include @@ -228,9 +228,9 @@ namespace gtsam { * Rot3::localCoordinates() */ enum CoordinatesMode { - FIRST_ORDER, ///< A fast first-order approximation to the exponential map. - SLOW_CALEY, ///< Retract and localCoordinates using the Caley transform. - CORRECT_EXPMAP ///< The correct exponential map, computationally expensive. + EXPMAP, ///< The exponential map, computationally expensive. + CALEY, ///< Retract and localCoordinates using the Caley transform. + SLOW_CALEY ///< Slow matrix implementation of Cayley transform (for tests only). }; /// dimension of the variable - used to autodetect sizes diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 658c66305..5f6f70c55 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -247,7 +247,9 @@ Vector Rot3::Logmap(const Rot3& R) { /* ************************************************************************* */ Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { - if(mode == Rot3::FIRST_ORDER) { + if(mode == Rot3::EXPMAP) { + return (*this)*Expmap(omega); + } else if(mode == Rot3::CALEY) { const double x = omega(0), y = omega(1), z = omega(2); const double x2 = x*x, y2 = y*y, z2 = z*z; const double xy = x*y, xz = x*z, yz = y*z; @@ -260,8 +262,6 @@ Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { } else if(mode == Rot3::SLOW_CALEY) { Matrix Omega = skewSymmetric(omega); return (*this)*Cayley<3>(-Omega/2); - } else if(mode == Rot3::CORRECT_EXPMAP) { - return (*this)*Expmap(omega); } else { assert(false); exit(1); @@ -270,7 +270,9 @@ Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { /* ************************************************************************* */ Vector Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const { - if(mode == Rot3::FIRST_ORDER) { + if(mode == Rot3::EXPMAP) { + return Logmap(between(T)); + } else if(mode == Rot3::CALEY) { // Create a fixed-size matrix Eigen::Matrix3d A(between(T).matrix()); // Mathematica closed form optimization (procrastination?) gone wild: @@ -290,8 +292,6 @@ Vector Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const { // using templated version of Cayley Matrix Omega = Cayley<3>(A); return -2*Vector_(3,Omega(2,1),Omega(0,2),Omega(1,0)); - } else if(mode == Rot3::CORRECT_EXPMAP) { - return Logmap(between(T)); } else { assert(false); exit(1); diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 6c6289844..4cc9c07f9 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -60,9 +60,9 @@ 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)); + EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v, Pose3::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)); + EXPECT(assert_equal(Pose3(R, P),id.retract(v, Pose3::EXPMAP),1e-2)); } /* ************************************************************************* */ @@ -461,8 +461,8 @@ TEST(Pose3, localCoordinates_first_order) 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))); + Pose3 t1 = T, t2 = t1.retract(d12, Pose3::EXPMAP); + EXPECT(assert_equal(d12, t1.localCoordinates(t2, Pose3::EXPMAP))); } /* ************************************************************************* */ @@ -483,10 +483,10 @@ 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))); + Vector d12 = t1.localCoordinates(t2, Pose3::EXPMAP); + EXPECT(assert_equal(t2, t1.retract(d12, Pose3::EXPMAP))); + Vector d21 = t2.localCoordinates(t1, Pose3::EXPMAP); + EXPECT(assert_equal(t1, t2.retract(d21, Pose3::EXPMAP))); // Check that log(t1,t2)=-log(t2,t1) EXPECT(assert_equal(d12,-d21)); diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index 177f35369..8311a62b5 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -201,17 +201,17 @@ TEST(Rot3, log) } /* ************************************************************************* */ -TEST(Rot3, manifold_first_order) +TEST(Rot3, manifold_caley) { Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2); Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7); Rot3 origin; // log behaves correctly - Vector d12 = gR1.localCoordinates(gR2, Rot3::FIRST_ORDER); - CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::FIRST_ORDER))); - Vector d21 = gR2.localCoordinates(gR1, Rot3::FIRST_ORDER); - CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::FIRST_ORDER))); + Vector d12 = gR1.localCoordinates(gR2, Rot3::CALEY); + CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::CALEY))); + Vector d21 = gR2.localCoordinates(gR1, Rot3::CALEY); + CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::CALEY))); // Check that log(t1,t2)=-log(t2,t1) CHECK(assert_equal(d12,-d21)); @@ -229,7 +229,7 @@ TEST(Rot3, manifold_first_order) } /* ************************************************************************* */ -TEST(Rot3, manifold_caley) +TEST(Rot3, manifold_slow_caley) { Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2); Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7); @@ -264,10 +264,10 @@ TEST(Rot3, manifold_expmap) Rot3 origin; // log behaves correctly - Vector d12 = gR1.localCoordinates(gR2, Rot3::CORRECT_EXPMAP); - CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::CORRECT_EXPMAP))); - Vector d21 = gR2.localCoordinates(gR1, Rot3::CORRECT_EXPMAP); - CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::CORRECT_EXPMAP))); + Vector d12 = gR1.localCoordinates(gR2, Rot3::EXPMAP); + CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::EXPMAP))); + Vector d21 = gR2.localCoordinates(gR1, Rot3::EXPMAP); + CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::EXPMAP))); // Check that it is expmap CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12)));