diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 0b88a70c7..3f681a433 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -175,7 +175,7 @@ Vector Rot3::quaternion() const { } /* ************************************************************************* */ -Matrix3 Rot3::rightJacobianExpMapSO3(const Vector3& x) { +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; @@ -183,15 +183,15 @@ Matrix3 Rot3::rightJacobianExpMapSO3(const Vector3& x) { 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 + 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; } /* ************************************************************************* */ -Matrix3 Rot3::rightJacobianExpMapSO3inverse(const Vector3& x) { +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; diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 6fe3f92b4..86a3f127f 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 @@ -287,15 +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) { - if(zero(v)) return Rot3(); - else return rodriguez(v); + 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); } /** * Log map at identity - return 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, boost::optional H = boost::none); /// Left-trivialized derivative of the exponential map static Matrix3 dexpL(const Vector3& v); @@ -306,13 +313,21 @@ namespace gtsam { /** * 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)) */ - static Matrix3 rightJacobianExpMapSO3(const Vector3& x); + 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) */ - static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x); + 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..2e21929f6 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -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, boost::optional 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,17 @@ 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->resize(3,3); + *H = Rot3::LogmapDerivative(thetaR); + } + return thetaR; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 26ca25bf2..2c56a2e0b 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -133,21 +133,22 @@ namespace gtsam { } /* ************************************************************************* */ - Vector3 Rot3::Logmap(const Rot3& R) { + Vector3 Rot3::Logmap(const Rot3& R, boost::optional 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,14 @@ namespace gtsam { angle -= twoPi; else if (angle < -M_PI) angle += twoPi; - return (angle / s) * q.vec(); + thetaR = (angle / s) * q.vec(); } + + if(H){ + H->resize(3,3); + *H = Rot3::LogmapDerivative(thetaR); + } + return thetaR; } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index c89d2dacb..6453063b5 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -214,33 +214,45 @@ 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) ) ); +/* ************************************************************************* */ +Vector3 thetahat(0.1, 0, 0.1); +TEST( Rot3, ExpmapDerivative ) +{ + Matrix Jexpected = numericalDerivative11( + boost::bind(&Rot3::Expmap, _1, boost::none), thetahat); + Matrix Jactual = Rot3::ExpmapDerivative(thetahat); + CHECK(assert_equal(Jexpected, Jactual)); } /* ************************************************************************* */ -TEST( Rot3, rightJacobianExpMapSO3 ) +TEST( Rot3, jacobianExpmap ) { - // Linearization point - Vector3 thetahat; thetahat << 0.1, 0, 0; - - Matrix expectedJacobian = numericalDerivative11( - boost::bind(&Rot3::Expmap, _1), thetahat); - Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat); - CHECK(assert_equal(expectedJacobian, actualJacobian)); + 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)); } /* ************************************************************************* */ -TEST( Rot3, rightJacobianExpMapSO3inverse ) +TEST( Rot3, LogmapDerivative ) { - // Linearization point - Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias - Vector3 deltatheta; deltatheta << 0, 0, 0; + 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)); +} - Matrix expectedJacobian = numericalDerivative11( - boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta); - Matrix actualJacobian = Rot3::rightJacobianExpMapSO3inverse(thetahat); - EXPECT(assert_equal(expectedJacobian, actualJacobian)); +/* ************************************************************************* */ +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)); } /* ************************************************************************* */