From 982ddaeecba1babb3ead6e8e3dbb4a0a32d89150 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 10 Feb 2015 20:14:59 +0100 Subject: [PATCH] Moved ExpmapDerivative and LogmapDerivative to SO3 --- gtsam/geometry/Rot3.cpp | 51 ++--------------------- gtsam/geometry/SO3.cpp | 92 +++++++++++++++++++++++++++++++++++------ gtsam/geometry/SO3.h | 74 +++++++++++++++++++++++++++------ 3 files changed, 145 insertions(+), 72 deletions(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index ab44716c8..fa09ddc21 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -19,6 +19,7 @@ */ #include +#include #include #include #include @@ -149,57 +150,13 @@ Vector Rot3::quaternion() const { } /* ************************************************************************* */ -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::ExpmapDerivative(const Vector3& x) { + return SO3::ExpmapDerivative(x); } /* ************************************************************************* */ 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 + return SO3::LogmapDerivative(x); } /* ************************************************************************* */ diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 08ae31945..5946a71eb 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -20,9 +20,12 @@ #include #include +using namespace std; + namespace gtsam { -SO3 Rodrigues(const double& theta, const Vector3& axis) { +/* ************************************************************************* */ +SO3 SO3::Rodrigues(const Vector3& axis, double theta) { using std::cos; using std::sin; @@ -46,27 +49,25 @@ SO3 Rodrigues(const double& theta, const Vector3& axis) { } /// simply convert omega to axis/angle representation -SO3 SO3::Expmap(const Eigen::Ref& omega, +SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { if (H) - CONCEPT_NOT_IMPLEMENTED; + *H = ExpmapDerivative(omega); if (omega.isZero()) - return SO3::Identity(); + return Identity(); else { double angle = omega.norm(); - return Rodrigues(angle, omega / angle); + return Rodrigues(omega / angle, angle); } } +/* ************************************************************************* */ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) { using std::sqrt; using std::sin; - if (H) - CONCEPT_NOT_IMPLEMENTED; - // note switch to base 1 const double& R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2); const double& R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2); @@ -75,16 +76,18 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) { // Get trace(R) double tr = R.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) { if (std::abs(R33 + 1.0) > 1e-10) - return (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33); + thetaR = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33); else if (std::abs(R22 + 1.0) > 1e-10) - return (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32); + thetaR = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32); else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit - return (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31); + thetaR = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31); } else { double magnitude; double tr_3 = tr - 3.0; // always negative @@ -96,9 +99,74 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) { // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2) magnitude = 0.5 - tr_3 * tr_3 / 12.0; } - return magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12); + thetaR = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12); } + + if(H) *H = LogmapDerivative(thetaR); + return thetaR; } +/* ************************************************************************* */ +Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { + using std::cos; + using std::sin; + + if(zero(omega)) return I_3x3; + double theta = omega.norm(); // rotation angle +#ifdef DUY_VERSION + /// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) + Matrix3 X = skewSymmetric(omega); + 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 = omega^, normalized by normx + const Matrix3 Y = skewSymmetric(omega) / theta; + return I_3x3 - ((1 - cos(theta)) / (theta)) * Y + + (1 - sin(theta) / theta) * Y * Y; // right Jacobian +#endif +} + +/* ************************************************************************* */ +Matrix3 SO3::LogmapDerivative(const Vector3& omega) { + using std::cos; + using std::sin; + + if(zero(omega)) return I_3x3; + double theta = omega.norm(); +#ifdef DUY_VERSION + /// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version) + Matrix3 X = skewSymmetric(omega); + 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(omega); // element of Lie algebra so(3): X = omega^ + return I_3x3 + 0.5 * X + + (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * X + * X; +#endif +} + +/* ************************************************************************* */ + } // end namespace gtsam diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 5f9bc37b0..a07c3601d 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -30,15 +30,21 @@ namespace gtsam { * We guarantee (all but first) constructors only generate from sub-manifold. * However, round-off errors in repeated composition could move off it... */ -class GTSAM_EXPORT SO3: public Matrix3, public LieGroup { +class GTSAM_EXPORT SO3: public Matrix3, public LieGroup { protected: public: - enum { dimension=3 }; + enum { + dimension = 3 + }; + + /// @name Constructors + /// @{ /// Constructor from AngleAxisd - SO3() : Matrix3(I_3x3) { + SO3() : + Matrix3(I_3x3) { } /// Constructor from Eigen Matrix @@ -52,6 +58,13 @@ public: Matrix3(angleAxis) { } + /// Static, named constructor TODO think about relation with above + static SO3 Rodrigues(const Vector3& axis, double theta); + + /// @} + /// @name Testable + /// @{ + void print(const std::string& s) const { std::cout << s << *this << std::endl; } @@ -60,32 +73,67 @@ public: return equal_with_abs_tol(*this, R, tol); } - static SO3 identity() { return I_3x3; } - SO3 inverse() const { return this->Matrix3::inverse(); } + /// @} + /// @name Group + /// @{ - static SO3 Expmap(const Eigen::Ref& omega, ChartJacobian H = boost::none); + /// identity rotation for group operation + static SO3 identity() { + return I_3x3; + } + + /// inverse of a rotation = transpose + SO3 inverse() const { + return this->Matrix3::inverse(); + } + + /// @} + /// @name Lie Group + /// @{ + + /** + * Exponential map at identity - create a rotation from canonical coordinates + * \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula + */ + static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none); + + /** + * Log map at identity - returns the canonical coordinates + * \f$ [R_x,R_y,R_z] \f$ of this rotation + */ static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none); - Matrix3 AdjointMap() const { return *this; } + /// Derivative of Expmap + static Matrix3 ExpmapDerivative(const Vector3& omega); + + /// Derivative of Logmap + static Matrix3 LogmapDerivative(const Vector3& omega); + + Matrix3 AdjointMap() const { + return *this; + } // Chart at origin struct ChartAtOrigin { - static SO3 Retract(const Vector3& v, ChartJacobian H = boost::none) { - return Expmap(v,H); + static SO3 Retract(const Vector3& omega, ChartJacobian H = boost::none) { + return Expmap(omega, H); } static Vector3 Local(const SO3& R, ChartJacobian H = boost::none) { - return Logmap(R,H); + return Logmap(R, H); } }; - using LieGroup::inverse; + using LieGroup::inverse; + /// @} }; template<> -struct traits : public internal::LieGroupTraits {}; +struct traits : public internal::LieGroupTraits { +}; template<> -struct traits : public internal::LieGroupTraits {}; +struct traits : public internal::LieGroupTraits { +}; } // end namespace gtsam