From e0a767e7fddca33fe65eba7f6a54d385db77fd43 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 24 Dec 2014 12:25:53 +0100 Subject: [PATCH] Renamed all dexpL/dexpInvL, merged Luca/Duy versions in Rot3 --- gtsam/base/LieScalar.h | 4 +- gtsam/geometry/Point2.h | 8 +-- gtsam/geometry/Point3.h | 4 +- gtsam/geometry/Pose2.cpp | 6 +- gtsam/geometry/Pose2.h | 4 +- gtsam/geometry/Rot2.h | 4 +- gtsam/geometry/Rot3.cpp | 97 ++++++++++++++---------------- gtsam/geometry/Rot3.h | 42 +++---------- gtsam/geometry/Rot3M.cpp | 9 +-- gtsam/geometry/Rot3Q.cpp | 9 +-- gtsam/geometry/tests/testPose2.cpp | 10 +-- gtsam/geometry/tests/testRot2.cpp | 6 +- gtsam/geometry/tests/testRot3.cpp | 48 ++++++++------- 13 files changed, 108 insertions(+), 143 deletions(-) diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index b208a584a..4e40e03a9 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -107,12 +107,12 @@ namespace gtsam { static Vector Logmap(const LieScalar& p) { return (Vector(1) << p.value()).finished(); } /// Left-trivialized derivative of the exponential map - static Matrix dexpL(const Vector& v) { + static Matrix ExpmapDerivative(const Vector& v) { return eye(1); } /// Left-trivialized derivative inverse of the exponential map - static Matrix dexpInvL(const Vector& v) { + static Matrix LogmapDerivative(const Vector& v) { return eye(1); } diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index fadea652b..da630a803 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -174,14 +174,10 @@ public: static inline Vector2 Logmap(const Point2& dp) { return Vector2(dp.x(), dp.y()); } /// Left-trivialized derivative of the exponential map - static Matrix dexpL(const Vector2& v) { - return eye(2); - } + static Matrix ExpmapDerivative(const Vector2& v) {return I_2x2;} /// Left-trivialized derivative inverse of the exponential map - static Matrix dexpInvL(const Vector2& v) { - return eye(2); - } + static Matrix LogmapDerivative(const Vector2& v) { return I_2x2;} /// @} /// @name Vector Space diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 56d9b8bef..0afe2acf8 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -142,12 +142,12 @@ namespace gtsam { static inline Vector3 Logmap(const Point3& dp) { return Vector3(dp.x(), dp.y(), dp.z()); } /// Left-trivialized derivative of the exponential map - static Matrix3 dexpL(const Vector& v) { + static Matrix3 ExpmapDerivative(const Vector& v) { return I_3x3; } /// Left-trivialized derivative inverse of the exponential map - static Matrix3 dexpInvL(const Vector& v) { + static Matrix3 LogmapDerivative(const Vector& v) { return I_3x3; } diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 719856f78..699994c3b 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -135,7 +135,7 @@ Matrix3 Pose2::adjointMap(const Vector& v) { } /* ************************************************************************* */ -Matrix3 Pose2::dexpL(const Vector3& v) { +Matrix3 Pose2::ExpmapDerivative(const Vector3& v) { double alpha = v[2]; if (fabs(alpha) > 1e-5) { // Chirikjian11book2, pg. 36 @@ -145,7 +145,7 @@ Matrix3 Pose2::dexpL(const Vector3& v) { * \dot{g} g^{-1} = dexpR_{q}\dot{q} * where q = A, and g = exp(A) * and the LHS is in the definition of J_l in Chirikjian11book2, pg. 26. - * Hence, to compute dexpL, we have to use the formula of J_r Chirikjian11book2, pg.36 + * Hence, to compute ExpmapDerivative, we have to use the formula of J_r Chirikjian11book2, pg.36 */ double sZalpha = sin(alpha)/alpha, c_1Zalpha = (cos(alpha)-1)/alpha; double v1Zalpha = v[0]/alpha, v2Zalpha = v[1]/alpha; @@ -164,7 +164,7 @@ Matrix3 Pose2::dexpL(const Vector3& v) { } /* ************************************************************************* */ -Matrix3 Pose2::dexpInvL(const Vector3& v) { +Matrix3 Pose2::LogmapDerivative(const Vector3& v) { double alpha = v[2]; if (fabs(alpha) > 1e-5) { double alphaInv = 1/alpha; diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index ba85ed0ac..aa201cfa2 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -182,10 +182,10 @@ public: } /// Left-trivialized derivative of the exponential map - static Matrix3 dexpL(const Vector3& v); + static Matrix3 ExpmapDerivative(const Vector3& v); /// Left-trivialized derivative inverse of the exponential map - static Matrix3 dexpInvL(const Vector3& v); + static Matrix3 LogmapDerivative(const Vector3& v); /// @} diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 21a402426..231522102 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -176,12 +176,12 @@ namespace gtsam { } /// Left-trivialized derivative of the exponential map - static Matrix dexpL(const Vector& v) { + static Matrix ExpmapDerivative(const Vector& v) { return ones(1); } /// Left-trivialized derivative inverse of the exponential map - static Matrix dexpInvL(const Vector& v) { + static Matrix LogmapDerivative(const Vector& v) { return ones(1); } diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 3f681a433..f494df07b 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -107,32 +107,6 @@ Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1, return q; } -/* ************************************************************************* */ -/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) -Matrix3 Rot3::dexpL(const Vector3& v) { - if(zero(v)) return eye(3); - Matrix3 x = skewSymmetric(v); - Matrix3 x2 = x*x; - double theta = v.norm(), vi = theta/2.0; - double s1 = sin(vi)/vi; - double s2 = (theta - sin(theta))/(theta*theta*theta); - Matrix3 res = I_3x3 - 0.5*s1*s1*x + s2*x2; - return res; -} - -/* ************************************************************************* */ -/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version) -Matrix3 Rot3::dexpInvL(const Vector3& v) { - if(zero(v)) return eye(3); - Matrix3 x = skewSymmetric(v); - Matrix3 x2 = x*x; - double theta = v.norm(), vi = theta/2.0; - double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta); - Matrix3 res = I_3x3 + 0.5*x - s2*x2; - return res; -} - - /* ************************************************************************* */ Point3 Rot3::column(int index) const{ if(index == 3) @@ -176,35 +150,56 @@ Vector Rot3::quaternion() const { /* ************************************************************************* */ Matrix3 Rot3::ExpmapDerivative(const Vector3& x) { - // x is the axis-angle representation (exponential coordinates) for a rotation - double normx = norm_2(x); // rotation angle - Matrix3 Jr; - if (normx < 10e-8){ - Jr = I_3x3; - } - else{ - const Matrix3 X = skewSymmetric(x) / normx; // element of Lie algebra so(3): X = x^, normalized by normx - Jr = I_3x3 - ((1-cos(normx))/(normx)) * X + - (1 -sin(normx)/normx) * X * X; // right Jacobian - } - return Jr; + if(zero(x)) return I_3x3; + double theta = x.norm(); // rotation angle +#ifdef DUY_VERSION + /// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) + Matrix3 X = skewSymmetric(x); + Matrix3 X2 = X*X; + double vi = theta/2.0; + double s1 = sin(vi)/vi; + double s2 = (theta - sin(theta))/(theta*theta*theta); + return I_3x3 - 0.5*s1*s1*X + s2*X2; +#else // Luca's version + /** + * Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in + * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. + * expmap(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega) + * where Jr = ExpmapDerivative(thetahat); + * This maps a perturbation in the tangent space (omega) to + * a perturbation on the manifold (expmap(Jr * omega)) + */ + // element of Lie algebra so(3): X = x^, normalized by normx + const Matrix3 Y = skewSymmetric(x) / theta; + return I_3x3 - ((1 - cos(theta)) / (theta)) * Y + + (1 - sin(theta) / theta) * Y * Y; // right Jacobian +#endif } /* ************************************************************************* */ Matrix3 Rot3::LogmapDerivative(const Vector3& x) { - // x is the axis-angle representation (exponential coordinates) for a rotation - double normx = norm_2(x); // rotation angle - Matrix3 Jrinv; - - if (normx < 10e-8){ - Jrinv = I_3x3; - } - else{ - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jrinv = I_3x3 + - 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; - } - return Jrinv; + if(zero(x)) return I_3x3; + double theta = x.norm(); +#ifdef DUY_VERSION + /// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version) + Matrix3 X = skewSymmetric(x); + Matrix3 X2 = X*X; + double vi = theta/2.0; + double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta); + return I_3x3 + 0.5*X - s2*X2; +#else // Luca's version + /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in + * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. + * logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega + * where Jrinv = LogmapDerivative(omega); + * This maps a perturbation on the manifold (expmap(omega)) + * to a perturbation in the tangent space (Jrinv * omega) + */ + const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ + return I_3x3 + 0.5 * X + + (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * X + * X; +#endif } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 86a3f127f..630dad356 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -216,7 +216,7 @@ namespace gtsam { } /// derivative of inverse rotation R^T s.t. inverse(R)*R = identity - Rot3 inverse(boost::optional H1=boost::none) const; + Rot3 inverse(OptionalJacobian<3,3> H1=boost::none) const; /// Compose two rotations i.e., R= (*this) * R2 Rot3 compose(const Rot3& R2, OptionalJacobian<3, 3> H1 = boost::none, @@ -288,45 +288,21 @@ namespace gtsam { * Exponential map at identity - create a rotation from canonical coordinates * \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula */ - static Rot3 Expmap(const Vector& v, boost::optional H = boost::none) { - if(H){ - H->resize(3,3); - *H = Rot3::ExpmapDerivative(v); - } - if(zero(v)) - return Rot3(); - else - return rodriguez(v); + static Rot3 Expmap(const Vector& v, OptionalJacobian<3,3> H = boost::none) { + if(H) *H = Rot3::ExpmapDerivative(v); + if (zero(v)) return Rot3(); else return rodriguez(v); } /** - * Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z] \f$ of this rotation + * Log map at identity - returns the canonical coordinates + * \f$ [R_x,R_y,R_z] \f$ of this rotation */ - static Vector3 Logmap(const Rot3& R, boost::optional H = boost::none); + static Vector3 Logmap(const Rot3& R, OptionalJacobian<3,3> H = boost::none); - /// Left-trivialized derivative of the exponential map - static Matrix3 dexpL(const Vector3& v); - - /// Left-trivialized derivative inverse of the exponential map - static Matrix3 dexpInvL(const Vector3& v); - - /** - * Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in - * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - * expmap(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega) - * where Jr = ExpmapDerivative(thetahat); - * This maps a perturbation in the tangent space (omega) to - * a perturbation on the manifold (expmap(Jr * omega)) - */ + /// Derivative of Expmap static Matrix3 ExpmapDerivative(const Vector3& x); - /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in - * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - * logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega - * where Jrinv = LogmapDerivative(omega); - * This maps a perturbation on the manifold (expmap(omega)) - * to a perturbation in the tangent space (Jrinv * omega) - */ + /// Derivative of Logmap static Matrix3 LogmapDerivative(const Vector3& x); /// @} diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 2e21929f6..041a1b854 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -158,7 +158,7 @@ Matrix3 Rot3::transpose() const { } /* ************************************************************************* */ -Rot3 Rot3::inverse(boost::optional H1) const { +Rot3 Rot3::inverse(OptionalJacobian<3,3> H1) const { if (H1) *H1 = -rot_; return Rot3(Matrix3(transpose())); } @@ -184,7 +184,7 @@ Point3 Rot3::rotate(const Point3& p, /* ************************************************************************* */ // Log map at identity - return the canonical coordinates of this rotation -Vector3 Rot3::Logmap(const Rot3& R, boost::optional H) { +Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) { static const double PI = boost::math::constants::pi(); @@ -223,10 +223,7 @@ Vector3 Rot3::Logmap(const Rot3& R, boost::optional H) { rot(1,0)-rot(0,1)); } - if(H){ - H->resize(3,3); - *H = Rot3::LogmapDerivative(thetaR); - } + if(H) *H = Rot3::LogmapDerivative(thetaR); return thetaR; } diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 2c56a2e0b..5c2158ab4 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -101,7 +101,7 @@ namespace gtsam { } /* ************************************************************************* */ - Rot3 Rot3::inverse(boost::optional H1) const { + Rot3 Rot3::inverse(OptionalJacobian<3,3> H1) const { if (H1) *H1 = -matrix(); return Rot3(quaternion_.inverse()); } @@ -133,7 +133,7 @@ namespace gtsam { } /* ************************************************************************* */ - Vector3 Rot3::Logmap(const Rot3& R, boost::optional H) { + Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) { using std::acos; using std::sqrt; static const double twoPi = 2.0 * M_PI, @@ -160,10 +160,7 @@ namespace gtsam { thetaR = (angle / s) * q.vec(); } - if(H){ - H->resize(3,3); - *H = Rot3::LogmapDerivative(thetaR); - } + if(H) *H = Rot3::LogmapDerivative(thetaR); return thetaR; } diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index da860b226..5d3f52516 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -204,22 +204,22 @@ Vector3 testDexpL(const Vector3 w, const Vector3& dw) { return y; } -TEST( Pose2, dexpL) { - Matrix actualDexpL = Pose2::dexpL(w); +TEST( Pose2, ExpmapDerivative) { + Matrix actualDexpL = Pose2::ExpmapDerivative(w); Matrix expectedDexpL = numericalDerivative11( boost::bind(testDexpL, w, _1), zero(3), 1e-2); EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); - Matrix actualDexpInvL = Pose2::dexpInvL(w); + Matrix actualDexpInvL = Pose2::LogmapDerivative(w); EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5)); // test case where alpha = 0 - Matrix actualDexpL0 = Pose2::dexpL(w0); + Matrix actualDexpL0 = Pose2::ExpmapDerivative(w0); Matrix expectedDexpL0 = numericalDerivative11( boost::bind(testDexpL, w0, _1), zero(3), 1e-2); EXPECT(assert_equal(expectedDexpL0, actualDexpL0, 1e-5)); - Matrix actualDexpInvL0 = Pose2::dexpInvL(w0); + Matrix actualDexpInvL0 = Pose2::LogmapDerivative(w0); EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5)); } diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 2b9e5c046..45fecb244 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -166,14 +166,14 @@ Vector1 testDexpL(const Vector& dw) { return y; } -TEST( Rot2, dexpL) { - Matrix actualDexpL = Rot2::dexpL(w); +TEST( Rot2, ExpmapDerivative) { + Matrix actualDexpL = Rot2::ExpmapDerivative(w); Matrix expectedDexpL = numericalDerivative11( boost::function( boost::bind(testDexpL, _1)), Vector(zero(1)), 1e-2); EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); - Matrix actualDexpInvL = Rot2::dexpInvL(w); + Matrix actualDexpInvL = Rot2::LogmapDerivative(w); EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5)); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 6453063b5..a8874fbfd 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -214,14 +214,39 @@ TEST(Rot3, log) CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI) } +/* ************************************************************************* */ +Vector w = Vector3(0.1, 0.27, -0.2); + +// Left trivialization Derivative of exp(w) wrpt w: +// How does exp(w) change when w changes? +// We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 +// => y = log (exp(-w) * exp(w+dw)) +Vector3 testDexpL(const Vector3& dw) { + return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw)); +} + +TEST( Rot3, ExpmapDerivative) { + Matrix actualDexpL = Rot3::ExpmapDerivative(w); + Matrix expectedDexpL = numericalDerivative11(testDexpL, + Vector3::Zero(), 1e-2); + EXPECT(assert_equal(expectedDexpL, actualDexpL,1e-7)); + + Matrix actualDexpInvL = Rot3::LogmapDerivative(w); + EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL,1e-7)); +} + /* ************************************************************************* */ Vector3 thetahat(0.1, 0, 0.1); -TEST( Rot3, ExpmapDerivative ) +TEST( Rot3, ExpmapDerivative2) { Matrix Jexpected = numericalDerivative11( boost::bind(&Rot3::Expmap, _1, boost::none), thetahat); + Matrix Jactual = Rot3::ExpmapDerivative(thetahat); CHECK(assert_equal(Jexpected, Jactual)); + + Matrix Jactual2 = Rot3::ExpmapDerivative(thetahat); + CHECK(assert_equal(Jexpected, Jactual2)); } /* ************************************************************************* */ @@ -412,27 +437,6 @@ TEST( Rot3, between ) CHECK(assert_equal(numericalH2,actualH2)); } -/* ************************************************************************* */ -Vector w = Vector3(0.1, 0.27, -0.2); - -// Left trivialization Derivative of exp(w) wrpt w: -// How does exp(w) change when w changes? -// We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 -// => y = log (exp(-w) * exp(w+dw)) -Vector3 testDexpL(const Vector3& dw) { - return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw)); -} - -TEST( Rot3, dexpL) { - Matrix actualDexpL = Rot3::dexpL(w); - Matrix expectedDexpL = numericalDerivative11(testDexpL, - Vector3::Zero(), 1e-2); - EXPECT(assert_equal(expectedDexpL, actualDexpL,1e-7)); - - Matrix actualDexpInvL = Rot3::dexpInvL(w); - EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL,1e-7)); -} - /* ************************************************************************* */ TEST( Rot3, xyz ) {