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