From 1e8402231c2115d70069c22bee3ba2a26b20b4ce Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 9 Dec 2014 16:59:30 -0500 Subject: [PATCH] applied (to some extend) the naming convention proposed by Frank --- gtsam/navigation/AHRSFactor.cpp | 38 ++++++++++---------- gtsam/navigation/CombinedImuFactor.cpp | 10 +++--- gtsam/navigation/ImuFactor.cpp | 10 +++--- gtsam/navigation/PreintegratedRotation.h | 8 ++--- gtsam/navigation/PreintegrationBase.h | 44 ++++++++++++------------ 5 files changed, 55 insertions(+), 55 deletions(-) diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 9c684658c..b85bf8e0b 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -83,11 +83,11 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( // rotation vector describing rotation increment computed from the // current rotation rate measurement const Vector3 theta_incr = correctedOmega * deltaT; - Matrix3 Jr_theta_incr; - const Rot3 incrR = Rot3::Expmap(theta_incr, Jr_theta_incr); // expensive !! + Matrix3 D_Rincr_integratedOmega; + const Rot3 incrR = Rot3::Expmap(theta_incr, D_Rincr_integratedOmega); // expensive !! // Update Jacobian - update_delRdelBiasOmega(Jr_theta_incr, incrR, deltaT); + update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT); // Update rotation and deltaTij. Matrix3 Fr; // Jacobian of the update @@ -172,44 +172,44 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj, boost::optional H2, boost::optional H3) const { // Do bias correction, if (H3) will contain 3*3 derivative used below - const Vector3 theta_biascorrected = _PIM_.predict(bias, H3); + const Vector3 biascorrectedOmega = _PIM_.predict(bias, H3); // Coriolis term const Vector3 coriolis = _PIM_.integrateCoriolis(Ri, omegaCoriolis_); const Matrix3 coriolisHat = skewSymmetric(coriolis); - const Vector3 theta_corrected = theta_biascorrected - coriolis; + const Vector3 correctedOmega = biascorrectedOmega - coriolis; // Prediction - const Rot3 deltaRij_corrected = Rot3::Expmap(theta_corrected); + const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); // Get error between actual and prediction const Rot3 actualRij = Ri.between(Rj); - const Rot3 fRhat = deltaRij_corrected.between(actualRij); - Vector3 fR = Rot3::Logmap(fRhat); + const Rot3 fRrot = correctedDeltaRij.between(actualRij); + Vector3 fR = Rot3::Logmap(fRrot); // Terms common to derivatives - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_corrected); - const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(fR); + const Matrix3 D_cDeltaRij_cOmega = Rot3::rightJacobianExpMapSO3(correctedOmega); + const Matrix3 D_fR_fRrot = Rot3::rightJacobianExpMapSO3inverse(fR); if (H1) { // dfR/dRi H1->resize(3, 3); - Matrix3 Jtheta = -Jr_theta_bcc * coriolisHat; + Matrix3 D_coriolis = -D_cDeltaRij_cOmega * coriolisHat; (*H1) - << Jrinv_fRhat * (-actualRij.transpose() - fRhat.transpose() * Jtheta); + << D_fR_fRrot * (-actualRij.transpose() - fRrot.transpose() * D_coriolis); } if (H2) { // dfR/dPosej H2->resize(3, 3); - (*H2) << Jrinv_fRhat * Matrix3::Identity(); + (*H2) << D_fR_fRrot * Matrix3::Identity(); } if (H3) { // dfR/dBias, note H3 contains derivative of predict - const Matrix3 JbiasOmega = Jr_theta_bcc * (*H3); + const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * (*H3); H3->resize(3, 3); - (*H3) << Jrinv_fRhat * (-fRhat.transpose() * JbiasOmega); + (*H3) << D_fR_fRrot * (-fRrot.transpose() * JbiasOmega); } Vector error(3); @@ -222,16 +222,16 @@ Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias, const PreintegratedMeasurements preintegratedMeasurements, const Vector3& omegaCoriolis, boost::optional body_P_sensor) { - const Vector3 theta_biascorrected = preintegratedMeasurements.predict(bias); + const Vector3 biascorrectedOmega = preintegratedMeasurements.predict(bias); // Coriolis term const Vector3 coriolis = // preintegratedMeasurements.integrateCoriolis(rot_i, omegaCoriolis); - const Vector3 theta_corrected = theta_biascorrected - coriolis; - const Rot3 deltaRij_corrected = Rot3::Expmap(theta_corrected); + const Vector3 correctedOmega = biascorrectedOmega - coriolis; + const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); - return rot_i.compose(deltaRij_corrected); + return rot_i.compose(correctedDeltaRij); } } //namespace gtsam diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 6ca1a7eb7..9aa4f080c 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -80,13 +80,13 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( Vector3 correctedAcc, correctedOmega; correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); - const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement - Matrix3 Jr_theta_incr; // Right jacobian computed at theta_incr - const Rot3 Rincr = Rot3::Expmap(theta_incr, Jr_theta_incr); // rotation increment computed from the current rotation rate measurement + const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement + Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr + const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement // Update Jacobians /* ----------------------------------------------------------------------------------------------------------------------- */ - updatePreintegratedJacobians(correctedAcc, Jr_theta_incr, Rincr, deltaT); + updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT); // 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. In this implementation, contrarily to [2] we @@ -99,7 +99,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // Single Jacobians to propagate covariance Matrix3 H_vel_biasacc = - R_i * deltaT; - Matrix3 H_angles_biasomega =- Jr_theta_incr * deltaT; + Matrix3 H_angles_biasomega =- D_Rincr_integratedOmega * deltaT; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix F(15,15); diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 1aa261d34..06d00b842 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -73,12 +73,12 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( Vector3 correctedAcc, correctedOmega; correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); - const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement - Matrix3 Jr_theta_incr; // Right jacobian computed at theta_incr - const Rot3 Rincr = Rot3::Expmap(theta_incr, Jr_theta_incr); // rotation increment computed from the current rotation rate measurement + const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement + Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr + const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement // Update Jacobians - updatePreintegratedJacobians(correctedAcc, Jr_theta_incr, Rincr, deltaT); + updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT); // Update preintegrated measurements (also get Jacobian) const Matrix3 R_i = deltaRij(); // store this, which is useful to compute G_test @@ -106,7 +106,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // intNoise accNoise omegaNoise (*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos Z_3x3, R_i * deltaT, Z_3x3, // vel - Z_3x3, Z_3x3, Jr_theta_incr * deltaT; // angle + Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle // Propagation with no approximation: // preintMeasCov = F * preintMeasCov * F.transpose() + G_test * (1/deltaT) * measurementCovariance * G_test.transpose(); } diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index d949ee21c..eb043fa79 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -84,10 +84,10 @@ public: * Update Jacobians to be used during preintegration * TODO: explain arguments */ - void update_delRdelBiasOmega(const Matrix3& Jr_theta_incr, const Rot3& incrR, + void update_delRdelBiasOmega(const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT) { const Matrix3 incrRt = incrR.transpose(); - delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - Jr_theta_incr * deltaT; + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_Rincr_integratedOmega * deltaT; } /// Return a bias corrected version of the integrated rotation - expensive @@ -104,11 +104,11 @@ public: if (H) { Matrix3 Jrinv_theta_bc; // This was done via an expmap, now we go *back* to so<3> - const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected, Jrinv_theta_bc); + const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, Jrinv_theta_bc); const Matrix3 Jr_JbiasOmegaIncr = // Rot3::rightJacobianExpMapSO3(delRdelBiasOmega_ * biasOmegaIncr); (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; - return theta_biascorrected; + return biascorrectedOmega; } //else return Rot3::Logmap(deltaRij_biascorrected); diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 11db97642..ee4d239a2 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -144,7 +144,7 @@ public: /// Update Jacobians to be used during preintegration void updatePreintegratedJacobians(const Vector3& correctedAcc, - const Matrix3& Jr_theta_incr, const Rot3& incrR, double deltaT){ + const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT){ Matrix3 dRij = deltaRij(); // expensive Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega(); if (!use2ndOrderIntegration_) { @@ -156,7 +156,7 @@ public: } delVdelBiasAcc_ += -dRij * deltaT; delVdelBiasOmega_ += temp; - update_delRdelBiasOmega(Jr_theta_incr,incrR,deltaT); + update_delRdelBiasOmega(D_Rincr_integratedOmega,incrR,deltaT); } void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, @@ -211,12 +211,12 @@ public: const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); // deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr) - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - Vector3 theta_corrected = theta_biascorrected - + Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected); + Vector3 correctedOmega = biascorrectedOmega - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term - const Rot3 deltaRij_corrected = - Rot3::Expmap( theta_corrected ); - const Rot3 Rot_j = Rot_i.compose( deltaRij_corrected ); + const Rot3 correctedDeltaRij = + Rot3::Expmap( correctedOmega ); + const Rot3 Rot_j = Rot_i.compose( correctedDeltaRij ); Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant @@ -246,31 +246,31 @@ public: // Get Get so<3> version of bias corrected rotation // If H5 is asked for, we will need the Jacobian, which we store in H5 // H5 will then be corrected below to take into account the Coriolis effect - Vector3 theta_biascorrected = biascorrectedThetaRij(biasOmegaIncr, H5); + Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5); // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration const Matrix3 omegaCoriolisHat = skewSymmetric(omegaCoriolis); const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis); - Vector3 theta_corrected = theta_biascorrected - coriolis; + Vector3 correctedOmega = biascorrectedOmega - coriolis; - Rot3 deltaRij_corrected, fRhat; + Rot3 correctedDeltaRij, fRrot; Vector3 fR; // Accessory matrix, used to build the jacobians - Matrix3 Jr_theta_bcc, Jtheta, Jrinv_fRhat; + Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot; // This is done to save computation: we ask for the jacobians only when they are needed if(H1 || H2 || H3 || H4 || H5){ - deltaRij_corrected = Rot3::Expmap( theta_corrected, Jr_theta_bcc); + correctedDeltaRij = Rot3::Expmap( correctedOmega, D_cDeltaRij_cOmega); // Residual rotation error - fRhat = deltaRij_corrected.between(Ri.between(Rj)); - fR = Rot3::Logmap(fRhat, Jrinv_fRhat); - Jtheta = -Jr_theta_bcc * skewSymmetric(coriolis); + fRrot = correctedDeltaRij.between(Ri.between(Rj)); + fR = Rot3::Logmap(fRrot, D_fR_fRrot); + D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis); }else{ - deltaRij_corrected = Rot3::Expmap( theta_corrected); + correctedDeltaRij = Rot3::Expmap( correctedOmega); // Residual rotation error - fRhat = deltaRij_corrected.between(Ri.between(Rj)); - fR = Rot3::Logmap(fRhat); + fRrot = correctedDeltaRij.between(Ri.between(Rj)); + fR = Rot3::Logmap(fRrot); } if(H1) { @@ -298,7 +298,7 @@ public: // dfV/dPi dfVdPi, // dfR/dRi - Jrinv_fRhat * (- Rj.between(Ri).matrix() - fRhat.inverse().matrix() * Jtheta), + D_fR_fRrot * (- Rj.between(Ri).matrix() - fRrot.inverse().matrix() * D_coriolis), // dfR/dPi Z_3x3; } @@ -322,7 +322,7 @@ public: // dfV/dPosej Matrix::Zero(3,6), // dfR/dPosej - Jrinv_fRhat * ( I_3x3 ), Z_3x3; + D_fR_fRrot * ( I_3x3 ), Z_3x3; } if(H4) { H4->resize(9,3); @@ -336,7 +336,7 @@ public: } if(H5) { // H5 by this point already contains 3*3 biascorrectedThetaRij derivative - const Matrix3 JbiasOmega = Jr_theta_bcc * (*H5); + const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * (*H5); H5->resize(9,6); (*H5) << // dfP/dBias @@ -347,7 +347,7 @@ public: - Ri.matrix() * delVdelBiasOmega(), // dfR/dBias Matrix::Zero(3,3), - Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); + D_fR_fRrot * ( - fRrot.inverse().matrix() * JbiasOmega); } // Evaluate residual error, according to [3]