diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 47b1cfdb7..6fba7a500 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -21,6 +21,10 @@ #pragma once +#ifndef ROT3_DEFAULT_COORDINATES_MODE +#define ROT3_DEFAULT_COORDINATES_MODE Rot3::FIRST_ORDER +#endif + #include #include @@ -220,6 +224,15 @@ namespace gtsam { /// @name Manifold /// @{ + /** Enum to indicate which method should be used in Rot3::retract() and + * 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. + }; + /// dimension of the variable - used to autodetect sizes static size_t Dim() { return dimension; } @@ -227,10 +240,10 @@ namespace gtsam { size_t dim() const { return dimension; } /// Retraction from R^3 to Pose2 manifold neighborhood around current pose - Rot3 retract(const Vector& omega) const; + Rot3 retract(const Vector& omega, CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const; /// Returns inverse retraction - Vector localCoordinates(const Rot3& t2) const; + Vector localCoordinates(const Rot3& t2, CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const; /// @} /// @name Lie Group diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index e547cfa1b..e9cc40c68 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -246,52 +246,56 @@ Vector Rot3::Logmap(const Rot3& R) { } /* ************************************************************************* */ -Rot3 Rot3::retract(const Vector& omega) const { -#ifdef CORRECT_ROT3_EXMAP - return (*this)*Expmap(omega); -#else -#ifdef SLOW_CAYLEY - Matrix Omega = skewSymmetric(omega); - return (*this)*Cayley<3>(-Omega/2); -#else - 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; - const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0*f; - return (*this)* Rot3( - (4+x2-y2-z2)*f, (xy - 2*z)*_2f, (xz + 2*y)*_2f, - (xy + 2*z)*_2f, (4-x2+y2-z2)*f, (yz - 2*x)*_2f, - (xz - 2*y)*_2f, (yz + 2*x)*_2f, (4-x2-y2+z2)*f - ); -#endif -#endif +Rot3 Rot3::retract(const Vector& omega, CoordinatesMode mode) const { + if(mode == Rot3::FIRST_ORDER) { + 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; + const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0*f; + return (*this)* Rot3( + (4+x2-y2-z2)*f, (xy - 2*z)*_2f, (xz + 2*y)*_2f, + (xy + 2*z)*_2f, (4-x2+y2-z2)*f, (yz - 2*x)*_2f, + (xz - 2*y)*_2f, (yz + 2*x)*_2f, (4-x2-y2+z2)*f + ); + } 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); + } } /* ************************************************************************* */ -Vector Rot3::localCoordinates(const Rot3& T) const { -#ifdef CORRECT_ROT3_EXMAP - return Logmap(between(T)); -#else - // Create a fixed-size matrix - Eigen::Matrix3d A(between(T).matrix()); -#ifdef SLOW_CAYLEY - // using templated version of Cayley - Matrix Omega = Cayley<3>(A); - return -2*Vector_(3,Omega(2,1),Omega(0,2),Omega(1,0)); -#else - // Mathematica closed form optimization (procrastination?) gone wild: - const double a=A(0,0),b=A(0,1),c=A(0,2); - const double d=A(1,0),e=A(1,1),f=A(1,2); - const double g=A(2,0),h=A(2,1),i=A(2,2); - const double di = d*i, ce = c*e, cd = c*d, fg=f*g; - const double M = 1 + e - f*h + i + e*i; - const double K = 2.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg)); - const double x = (a * f - cd + f) * K; - const double y = (b * f - ce - c) * K; - const double z = (fg - di - d) * K; - return -2 * Vector_(3, x, y, z); -#endif -#endif +Vector Rot3::localCoordinates(const Rot3& T, CoordinatesMode mode) const { + if(mode == Rot3::FIRST_ORDER) { + // Create a fixed-size matrix + Eigen::Matrix3d A(between(T).matrix()); + // Mathematica closed form optimization (procrastination?) gone wild: + const double a=A(0,0),b=A(0,1),c=A(0,2); + const double d=A(1,0),e=A(1,1),f=A(1,2); + const double g=A(2,0),h=A(2,1),i=A(2,2); + const double di = d*i, ce = c*e, cd = c*d, fg=f*g; + const double M = 1 + e - f*h + i + e*i; + const double K = 2.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg)); + const double x = (a * f - cd + f) * K; + const double y = (b * f - ce - c) * K; + const double z = (fg - di - d) * K; + return -2 * Vector_(3, x, y, z); + } else if(mode == Rot3::SLOW_CALEY) { + // Create a fixed-size matrix + Eigen::Matrix3d A(between(T).matrix()); + // 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/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index a51802afa..b703bbf48 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -150,12 +150,12 @@ namespace gtsam { } /* ************************************************************************* */ - Rot3 Rot3::retract(const Vector& omega) const { + Rot3 Rot3::retract(const Vector& omega, CoordinatesMode mode) const { return compose(Expmap(omega)); } /* ************************************************************************* */ - Vector Rot3::localCoordinates(const Rot3& t2) const { + Vector Rot3::localCoordinates(const Rot3& t2, CoordinatesMode mode) const { return Logmap(between(t2)); } diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index 6dacc75c9..177f35369 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -201,23 +201,17 @@ TEST(Rot3, log) } /* ************************************************************************* */ -TEST(Rot3, manifold) +TEST(Rot3, manifold_first_order) { 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); - CHECK(assert_equal(gR2, gR1.retract(d12))); - Vector d21 = gR2.localCoordinates(gR1); - CHECK(assert_equal(gR1, gR2.retract(d21))); - -#ifdef CORRECT_ROT3_EXMAP - // Check that it is expmap - CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12))); - CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21))); -#endif + 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))); // Check that log(t1,t2)=-log(t2,t1) CHECK(assert_equal(d12,-d21)); @@ -234,6 +228,66 @@ TEST(Rot3, manifold) CHECK(assert_equal(R5,R3*R2)); } +/* ************************************************************************* */ +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::SLOW_CALEY); + CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::SLOW_CALEY))); + Vector d21 = gR2.localCoordinates(gR1, Rot3::SLOW_CALEY); + CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::SLOW_CALEY))); + + // Check that log(t1,t2)=-log(t2,t1) + CHECK(assert_equal(d12,-d21)); + + // lines in canonical coordinates correspond to Abelian subgroups in SO(3) + Vector d = Vector_(3, 0.1, 0.2, 0.3); + // exp(-d)=inverse(exp(d)) + CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); + // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) + Rot3 R2 = Rot3::Expmap (2 * d); + Rot3 R3 = Rot3::Expmap (3 * d); + Rot3 R5 = Rot3::Expmap (5 * d); + CHECK(assert_equal(R5,R2*R3)); + CHECK(assert_equal(R5,R3*R2)); +} + +/* ************************************************************************* */ +TEST(Rot3, manifold_expmap) +{ + 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::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))); + + // Check that it is expmap + CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12))); + CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21))); + + // Check that log(t1,t2)=-log(t2,t1) + CHECK(assert_equal(d12,-d21)); + + // lines in canonical coordinates correspond to Abelian subgroups in SO(3) + Vector d = Vector_(3, 0.1, 0.2, 0.3); + // exp(-d)=inverse(exp(d)) + CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); + // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) + Rot3 R2 = Rot3::Expmap (2 * d); + Rot3 R3 = Rot3::Expmap (3 * d); + Rot3 R5 = Rot3::Expmap (5 * d); + CHECK(assert_equal(R5,R2*R3)); + CHECK(assert_equal(R5,R3*R2)); +} + /* ************************************************************************* */ class AngularVelocity: public Point3 { public: @@ -498,8 +552,6 @@ TEST(Rot3, quaternion) { EXPECT(assert_equal(expected2, actual2)); } -#endif - /* ************************************************************************* */ TEST( Rot3, Cayley ) { Matrix A = skewSymmetric(1,2,-3); @@ -508,6 +560,8 @@ TEST( Rot3, Cayley ) { EXPECT(assert_equal(A, Cayley(Q))); } +#endif + /* ************************************************************************* */ int main() { TestResult tr;