From e82b815a48484f35e133efc1fdf44acf2b1e3c70 Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 9 Dec 2014 17:10:04 -0500 Subject: [PATCH] renamed right jacobian of expmap and logmap (removed "right", according to Frank's suggestion :-) --- gtsam/geometry/Rot3.cpp | 4 ++-- gtsam/geometry/Rot3.h | 11 ++++++----- gtsam/geometry/Rot3M.cpp | 2 +- gtsam/geometry/Rot3Q.cpp | 2 +- gtsam/geometry/tests/testRot3.cpp | 8 ++++---- gtsam/navigation/AHRSFactor.cpp | 4 ++-- gtsam/navigation/PreintegratedRotation.h | 2 +- gtsam/navigation/tests/testAHRSFactor.cpp | 4 ++-- gtsam/navigation/tests/testImuFactor.cpp | 6 +++--- 9 files changed, 22 insertions(+), 21 deletions(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 846e0e070..43762fc54 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -195,7 +195,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; @@ -211,7 +211,7 @@ Matrix3 Rot3::rightJacobianExpMapSO3(const Vector3& x) { } /* ************************************************************************* */ -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 87a035bfb..93a32302b 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 @@ -297,7 +298,7 @@ namespace gtsam { static Rot3 Expmap(const Vector& v, boost::optional H = boost::none) { if(H){ H->resize(3,3); - *H = Rot3::rightJacobianExpMapSO3(v); + *H = Rot3::expmapDerivative(v); } if(zero(v)) return Rot3(); @@ -320,20 +321,20 @@ 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 = rightJacobianExpMapSO3(thetahat); + * 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 = rightJacobianExpMapSO3inverse(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 46f5b8109..60e5a48e1 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -241,7 +241,7 @@ Vector3 Rot3::Logmap(const Rot3& R, boost::optional H) { if(H){ H->resize(3,3); - *H = Rot3::rightJacobianExpMapSO3inverse(thetaR); + *H = Rot3::logmapDerivative(thetaR); } return thetaR; } diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index e73ed3eaf..7fa7426cf 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -175,7 +175,7 @@ namespace gtsam { if(H){ H->resize(3,3); - *H = Rot3::rightJacobianExpMapSO3inverse(thetaR); + *H = Rot3::logmapDerivative(thetaR); } return thetaR; } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 824878d13..f348c8d3c 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -217,11 +217,11 @@ TEST(Rot3, log) /* ************************************************************************* */ Vector3 thetahat(0.1, 0, 0.1); -TEST( Rot3, rightJacobianExpMapSO3 ) +TEST( Rot3, expmapDerivative ) { Matrix Jexpected = numericalDerivative11( boost::bind(&Rot3::Expmap, _1, boost::none), thetahat); - Matrix Jactual = Rot3::rightJacobianExpMapSO3(thetahat); + Matrix Jactual = Rot3::expmapDerivative(thetahat); CHECK(assert_equal(Jexpected, Jactual)); } @@ -236,12 +236,12 @@ TEST( Rot3, jacobianExpmap ) } /* ************************************************************************* */ -TEST( Rot3, rightJacobianExpMapSO3inverse ) +TEST( Rot3, logmapDerivative ) { Rot3 R = Rot3::Expmap(thetahat); // some rotation Matrix Jexpected = numericalDerivative11(boost::bind( &Rot3::Logmap, _1, boost::none), R); - Matrix3 Jactual = Rot3::rightJacobianExpMapSO3inverse(thetahat); + Matrix3 Jactual = Rot3::logmapDerivative(thetahat); EXPECT(assert_equal(Jexpected, Jactual)); } diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index b85bf8e0b..22c1c19ac 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -188,8 +188,8 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj, Vector3 fR = Rot3::Logmap(fRrot); // Terms common to derivatives - const Matrix3 D_cDeltaRij_cOmega = Rot3::rightJacobianExpMapSO3(correctedOmega); - const Matrix3 D_fR_fRrot = Rot3::rightJacobianExpMapSO3inverse(fR); + const Matrix3 D_cDeltaRij_cOmega = Rot3::expmapDerivative(correctedOmega); + const Matrix3 D_fR_fRrot = Rot3::logmapDerivative(fR); if (H1) { // dfR/dRi diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index eb043fa79..019b35ed9 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -106,7 +106,7 @@ public: // This was done via an expmap, now we go *back* to so<3> const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, Jrinv_theta_bc); const Matrix3 Jr_JbiasOmegaIncr = // - Rot3::rightJacobianExpMapSO3(delRdelBiasOmega_ * biasOmegaIncr); + Rot3::expmapDerivative(delRdelBiasOmega_ * biasOmegaIncr); (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; return biascorrectedOmega; } diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index e6ecf42a3..3605190d8 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 29a2a13de..c496cfb6a 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -340,7 +340,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 @@ -361,7 +361,7 @@ TEST( ImuFactor, PartialDerivativeLogmap ) Matrix expectedDelFdeltheta = numericalDerivative11(boost::bind( &evaluateLogRotation, thetahat, _1), Vector3(deltatheta)); - Matrix3 actualDelFdeltheta = Rot3::rightJacobianExpMapSO3inverse(thetahat); + Matrix3 actualDelFdeltheta = Rot3::logmapDerivative(thetahat); // Compare Jacobians EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta)); @@ -399,7 +399,7 @@ TEST( ImuFactor, fistOrderExponential ) double alpha = 0.0; 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