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/base/chartTesting.h b/gtsam/base/chartTesting.h index d2f453521..b1b47f020 100644 --- a/gtsam/base/chartTesting.h +++ b/gtsam/base/chartTesting.h @@ -39,12 +39,6 @@ void testDefaultChart(TestResult& result_, BOOST_CONCEPT_ASSERT((ChartConcept)); T other = value; - // Check for the existence of a print function. - gtsam::traits::print()(value, "value"); - gtsam::traits::print()(other, "other"); - - // Check for the existence of "equals" - EXPECT(gtsam::traits::equals()(value, other, 1e-12)); // Check that the dimension of the local value matches the chart dimension. Vector dx = Chart::local(value, other); 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 0b88a70c7..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) @@ -175,36 +149,57 @@ Vector Rot3::quaternion() const { } /* ************************************************************************* */ -Matrix3 Rot3::rightJacobianExpMapSO3(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); // element of Lie algebra so(3): X = x^ - Jr = I_3x3 - ((1-cos(normx))/(normx*normx)) * X + - ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian - } - return Jr; +Matrix3 Rot3::ExpmapDerivative(const Vector3& x) { + 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::rightJacobianExpMapSO3inverse(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; +Matrix3 Rot3::LogmapDerivative(const Vector3& x) { + 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 6fe3f92b4..630dad356 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -16,6 +16,7 @@ * @author Christian Potthast * @author Frank Dellaert * @author Richard Roberts + * @author Luca Carlone */ // \callgraph @@ -215,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, @@ -287,32 +288,22 @@ 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) { - 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); + 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); + /// Derivative of Expmap + static Matrix3 ExpmapDerivative(const Vector3& x); - /// 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. - */ - static Matrix3 rightJacobianExpMapSO3(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. - */ - static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x); + /// Derivative of Logmap + static Matrix3 LogmapDerivative(const Vector3& x); /// @} /// @name Group Action on Point3 diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index fc3490fb5..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) { +Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) { static const double PI = boost::math::constants::pi(); @@ -192,6 +192,8 @@ Vector3 Rot3::Logmap(const Rot3& R) { // Get trace(R) double tr = rot.trace(); + Vector3 thetaR; + // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // we do something special if (std::abs(tr+1.0) < 1e-10) { @@ -202,7 +204,7 @@ Vector3 Rot3::Logmap(const Rot3& R) { return (PI / sqrt(2.0+2.0*rot(1,1))) * Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1)); else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit - return (PI / sqrt(2.0+2.0*rot(0,0))) * + thetaR = (PI / sqrt(2.0+2.0*rot(0,0))) * Vector3(1.0+rot(0,0), rot(1,0), rot(2,0)); } else { double magnitude; @@ -215,11 +217,14 @@ Vector3 Rot3::Logmap(const Rot3& R) { // use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2) magnitude = 0.5 - tr_3*tr_3/12.0; } - return magnitude*Vector3( + thetaR = magnitude*Vector3( rot(2,1)-rot(1,2), rot(0,2)-rot(2,0), rot(1,0)-rot(0,1)); } + + if(H) *H = Rot3::LogmapDerivative(thetaR); + return thetaR; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 26ca25bf2..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,21 +133,22 @@ namespace gtsam { } /* ************************************************************************* */ - Vector3 Rot3::Logmap(const Rot3& R) { + Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) { using std::acos; using std::sqrt; static const double twoPi = 2.0 * M_PI, // define these compile time constants to avoid std::abs: - NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10; + NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10; + Vector3 thetaR; const Quaternion& q = R.quaternion_; const double qw = q.w(); if (qw > NearlyOne) { // Taylor expansion of (angle / s) at 1 - return (2 - 2 * (qw - 1) / 3) * q.vec(); + thetaR = (2 - 2 * (qw - 1) / 3) * q.vec(); } else if (qw < NearlyNegativeOne) { // Angle is zero, return zero vector - return Vector3::Zero(); + thetaR = Vector3::Zero(); } else { // Normal, away from zero case double angle = 2 * acos(qw), s = sqrt(1 - qw * qw); @@ -156,8 +157,11 @@ namespace gtsam { angle -= twoPi; else if (angle < -M_PI) angle += twoPi; - return (angle / s) * q.vec(); + thetaR = (angle / s) * q.vec(); } + + 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 c89d2dacb..a8874fbfd 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -214,33 +214,70 @@ TEST(Rot3, log) CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI) } -Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){ - return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) ); +/* ************************************************************************* */ +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)); } /* ************************************************************************* */ -TEST( Rot3, rightJacobianExpMapSO3 ) +Vector3 thetahat(0.1, 0, 0.1); +TEST( Rot3, ExpmapDerivative2) { - // Linearization point - Vector3 thetahat; thetahat << 0.1, 0, 0; + Matrix Jexpected = numericalDerivative11( + boost::bind(&Rot3::Expmap, _1, boost::none), thetahat); - Matrix expectedJacobian = numericalDerivative11( - boost::bind(&Rot3::Expmap, _1), thetahat); - Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat); - CHECK(assert_equal(expectedJacobian, actualJacobian)); + Matrix Jactual = Rot3::ExpmapDerivative(thetahat); + CHECK(assert_equal(Jexpected, Jactual)); + + Matrix Jactual2 = Rot3::ExpmapDerivative(thetahat); + CHECK(assert_equal(Jexpected, Jactual2)); } /* ************************************************************************* */ -TEST( Rot3, rightJacobianExpMapSO3inverse ) +TEST( Rot3, jacobianExpmap ) { - // Linearization point - Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias - Vector3 deltatheta; deltatheta << 0, 0, 0; + Matrix Jexpected = numericalDerivative11(boost::bind( + &Rot3::Expmap, _1, boost::none), thetahat); + Matrix3 Jactual; + const Rot3 R = Rot3::Expmap(thetahat, Jactual); + EXPECT(assert_equal(Jexpected, Jactual)); +} - Matrix expectedJacobian = numericalDerivative11( - boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta); - Matrix actualJacobian = Rot3::rightJacobianExpMapSO3inverse(thetahat); - EXPECT(assert_equal(expectedJacobian, actualJacobian)); +/* ************************************************************************* */ +TEST( Rot3, LogmapDerivative ) +{ + Rot3 R = Rot3::Expmap(thetahat); // some rotation + Matrix Jexpected = numericalDerivative11(boost::bind( + &Rot3::Logmap, _1, boost::none), R); + Matrix3 Jactual = Rot3::LogmapDerivative(thetahat); + EXPECT(assert_equal(Jexpected, Jactual)); +} + +/* ************************************************************************* */ +TEST( Rot3, jacobianLogmap ) +{ + Rot3 R = Rot3::Expmap(thetahat); // some rotation + Matrix Jexpected = numericalDerivative11(boost::bind( + &Rot3::Logmap, _1, boost::none), R); + Matrix3 Jactual; + Rot3::Logmap(R, Jactual); + EXPECT(assert_equal(Jexpected, Jactual)); } /* ************************************************************************* */ @@ -400,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 ) { diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 137f102b3..195490e87 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -99,7 +99,7 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( const Matrix3 incrRt = incrR.transpose(); // Right Jacobian computed at theta_incr - const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); + const Matrix3 Jr_theta_incr = Rot3::ExpmapDerivative(theta_incr); // Update Jacobians // --------------------------------------------------------------------------- @@ -108,11 +108,11 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( // Update preintegrated measurements covariance // --------------------------------------------------------------------------- const Vector3 theta_i = Rot3::Logmap(deltaRij_); // Parameterization of so(3) - const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_i); + const Matrix3 Jr_theta_i = Rot3::LogmapDerivative(theta_i); Rot3 Rot_j = deltaRij_ * incrR; const Vector3 theta_j = Rot3::Logmap(Rot_j); // Parameterization of so(3) - const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); + const Matrix3 Jrinv_theta_j = Rot3::LogmapDerivative(theta_j); // Update preintegrated measurements covariance: as in [2] we consider a first // order propagation that can be seen as a prediction phase in an EKF framework @@ -145,9 +145,9 @@ Vector3 AHRSFactor::PreintegratedMeasurements::predict(const Vector3& bias, const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); if (H) { const Matrix3 Jrinv_theta_bc = // - Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); + Rot3::LogmapDerivative(theta_biascorrected); const Matrix3 Jr_JbiasOmegaIncr = // - Rot3::rightJacobianExpMapSO3(delRdelBiasOmega_biasOmegaIncr); + Rot3::ExpmapDerivative(delRdelBiasOmega_biasOmegaIncr); (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; } return theta_biascorrected; @@ -238,8 +238,8 @@ Vector AHRSFactor::evaluateError(const Rot3& rot_i, const Rot3& rot_j, Vector3 fR = Rot3::Logmap(fRhat); // Terms common to derivatives - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_corrected); - const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(fR); + const Matrix3 Jr_theta_bcc = Rot3::ExpmapDerivative(theta_corrected); + const Matrix3 Jrinv_fRhat = Rot3::LogmapDerivative(fR); if (H1) { // dfR/dRi diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index a2ce631ea..10f12ec66 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -116,7 +116,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement - const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr + const Matrix3 Jr_theta_incr = Rot3::ExpmapDerivative(theta_incr); // Right jacobian computed at theta_incr // Update Jacobians /* ----------------------------------------------------------------------------------------------------------------------- */ @@ -138,11 +138,11 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements /* ----------------------------------------------------------------------------------------------------------------------- */ const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) - const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); + const Matrix3 Jr_theta_i = Rot3::ExpmapDerivative(theta_i); Rot3 Rot_j = deltaRij_ * Rincr; const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) - const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); + const Matrix3 Jrinv_theta_j = Rot3::LogmapDerivative(theta_j); // Single Jacobians to propagate covariance Matrix3 H_pos_pos = I_3x3; @@ -293,11 +293,11 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_ const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); + const Matrix3 Jr_theta_bcc = Rot3::ExpmapDerivative(theta_biascorrected_corioliscorrected); const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); - const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); + const Matrix3 Jrinv_fRhat = Rot3::LogmapDerivative(Rot3::Logmap(fRhat)); if(H1) { H1->resize(15,6); @@ -382,8 +382,8 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_ } if(H5) { - const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); + const Matrix3 Jrinv_theta_bc = Rot3::LogmapDerivative(theta_biascorrected); + const Matrix3 Jr_JbiasOmegaIncr = Rot3::ExpmapDerivative(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; H5->resize(15,6); diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 9a9cc9d62..cdebffa63 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -113,7 +113,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement - const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr + const Matrix3 Jr_theta_incr = Rot3::ExpmapDerivative(theta_incr); // Right jacobian computed at theta_incr // Update Jacobians /* ----------------------------------------------------------------------------------------------------------------------- */ @@ -133,11 +133,11 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework /* ----------------------------------------------------------------------------------------------------------------------- */ const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) - const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); + const Matrix3 Jr_theta_i = Rot3::ExpmapDerivative(theta_i); Rot3 Rot_j = deltaRij_ * Rincr; const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) - const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); + const Matrix3 Jrinv_theta_j = Rot3::LogmapDerivative(theta_j); Matrix H_pos_pos = I_3x3; Matrix H_pos_vel = I_3x3 * deltaT; @@ -275,11 +275,11 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); + const Matrix3 Jr_theta_bcc = Rot3::ExpmapDerivative(theta_biascorrected_corioliscorrected); const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); - const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); + const Matrix3 Jrinv_fRhat = Rot3::LogmapDerivative(Rot3::Logmap(fRhat)); if(H1) { H1->resize(9,6); @@ -348,8 +348,8 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const } if(H5) { - const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); + const Matrix3 Jrinv_theta_bc = Rot3::LogmapDerivative(theta_biascorrected); + const Matrix3 Jr_JbiasOmegaIncr = Rot3::ExpmapDerivative(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; H5->resize(9,6); diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index e6ecf42a3..99952f99e 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -241,7 +241,7 @@ TEST( AHRSFactor, PartialDerivativeExpmap ) { Matrix expectedDelRdelBiasOmega = numericalDerivative11( boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), biasOmega); - const Matrix3 Jr = Rot3::rightJacobianExpMapSO3( + const Matrix3 Jr = Rot3::ExpmapDerivative( (measuredOmega - biasOmega) * deltaT); Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign @@ -293,7 +293,7 @@ TEST( AHRSFactor, fistOrderExponential ) { Vector3 deltabiasOmega; deltabiasOmega << alpha, alpha, alpha; - const Matrix3 Jr = Rot3::rightJacobianExpMapSO3( + const Matrix3 Jr = Rot3::ExpmapDerivative( (measuredOmega - biasOmega) * deltaT); Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index cf81c32ae..06e2c105e 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -307,7 +307,7 @@ TEST( ImuFactor, PartialDerivativeExpmap ) Matrix expectedDelRdelBiasOmega = numericalDerivative11(boost::bind( &evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega)); - const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); Matrix3 actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign @@ -355,7 +355,7 @@ TEST( ImuFactor, fistOrderExponential ) Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha; - const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 05260521e..4691da384 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -45,7 +45,7 @@ static boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, static double rankTol = 1.0; static double linThreshold = -1.0; -static bool manageDegeneracy = true; +// static bool manageDegeneracy = true; // Create a noise model for the pixel error static SharedNoiseModel model(noiseModel::Unit::Create(3));