From b3f0f3877c5928b7d9fe1d6ba60ec2c23f610a6b Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 10 Dec 2014 16:16:29 -0500 Subject: [PATCH] capitalized ExpmapDerivative and LogmapDerivative --- gtsam/geometry/Rot3.cpp | 4 ++-- gtsam/geometry/Rot3.h | 10 +++++----- 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, 21 insertions(+), 21 deletions(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index dea76b244..bb6deafc9 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -175,7 +175,7 @@ Vector Rot3::quaternion() const { } /* ************************************************************************* */ -Matrix3 Rot3::expmapDerivative(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; @@ -191,7 +191,7 @@ Matrix3 Rot3::expmapDerivative(const Vector3& x) { } /* ************************************************************************* */ -Matrix3 Rot3::logmapDerivative(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 7a8c8b236..86a3f127f 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -291,7 +291,7 @@ namespace gtsam { static Rot3 Expmap(const Vector& v, boost::optional H = boost::none) { if(H){ H->resize(3,3); - *H = Rot3::expmapDerivative(v); + *H = Rot3::ExpmapDerivative(v); } if(zero(v)) return Rot3(); @@ -314,20 +314,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 = expmapDerivative(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 expmapDerivative(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); + * where Jrinv = LogmapDerivative(omega); * This maps a perturbation on the manifold (expmap(omega)) * to a perturbation in the tangent space (Jrinv * omega) */ - static Matrix3 logmapDerivative(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 9169c46ba..2e21929f6 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -225,7 +225,7 @@ Vector3 Rot3::Logmap(const Rot3& R, boost::optional H) { if(H){ H->resize(3,3); - *H = Rot3::logmapDerivative(thetaR); + *H = Rot3::LogmapDerivative(thetaR); } return thetaR; } diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index dc50a9687..2c56a2e0b 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -162,7 +162,7 @@ namespace gtsam { if(H){ H->resize(3,3); - *H = Rot3::logmapDerivative(thetaR); + *H = Rot3::LogmapDerivative(thetaR); } return thetaR; } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 08f7681a8..6453063b5 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -216,11 +216,11 @@ TEST(Rot3, log) /* ************************************************************************* */ Vector3 thetahat(0.1, 0, 0.1); -TEST( Rot3, expmapDerivative ) +TEST( Rot3, ExpmapDerivative ) { Matrix Jexpected = numericalDerivative11( boost::bind(&Rot3::Expmap, _1, boost::none), thetahat); - Matrix Jactual = Rot3::expmapDerivative(thetahat); + Matrix Jactual = Rot3::ExpmapDerivative(thetahat); CHECK(assert_equal(Jexpected, Jactual)); } @@ -235,12 +235,12 @@ TEST( Rot3, jacobianExpmap ) } /* ************************************************************************* */ -TEST( Rot3, logmapDerivative ) +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); + Matrix3 Jactual = Rot3::LogmapDerivative(thetahat); EXPECT(assert_equal(Jexpected, Jactual)); } diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 22c1c19ac..2bf49d9df 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::expmapDerivative(correctedOmega); - const Matrix3 D_fR_fRrot = Rot3::logmapDerivative(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 36bc9aabb..c576acc38 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::expmapDerivative(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 3605190d8..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::expmapDerivative( + 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::expmapDerivative( + 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 c496cfb6a..600bb4e11 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::expmapDerivative((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::logmapDerivative(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::expmapDerivative((measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign