From f991c1a3989c407a5b4bb4d04eb6677fcb7ad5f6 Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 8 Dec 2014 13:15:51 -0500 Subject: [PATCH] getting rid of rightJacobianSO3 (not completed yet) --- gtsam/navigation/ImuFactor.cpp | 13 +++++++----- gtsam/navigation/PreintegratedRotation.h | 15 +++++++------- gtsam/navigation/PreintegrationBase.h | 25 ++++++++++++++---------- 3 files changed, 31 insertions(+), 22 deletions(-) diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 4cd5977fb..f25268350 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -77,8 +77,8 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( 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 - 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 + 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 // Update Jacobians /* ----------------------------------------------------------------------------------------------------------------------- */ @@ -87,15 +87,18 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // 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 /* ----------------------------------------------------------------------------------------------------------------------- */ +// Matrix3 Jr_theta_i; +// const Vector3 theta_i = thetaRij(Jr_theta_i); // super-expensive parametrization of so(3) +// const Matrix3 R_i = deltaRij(); + const Vector3 theta_i = thetaRij(); // super-expensive parametrization of so(3) - const Matrix3 R_i = deltaRij(); const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); + const Matrix3 R_i = deltaRij(); // Update preintegrated measurements updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT); - const Vector3 theta_j = thetaRij(); // super-expensive parametrization of so(3) - const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); + Matrix3 Jrinv_theta_j; thetaRij(Jrinv_theta_j); // computation of rightJacobianInverse Matrix H_vel_angles = - R_i * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 0922ea63d..b93360fab 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -49,7 +49,7 @@ public: /// methods to access class variables Matrix3 deltaRij() const {return deltaRij_.matrix();} // expensive - Vector3 thetaRij() const {return Rot3::Logmap(deltaRij_);} // super-expensive + Vector3 thetaRij(boost::optional H = boost::none) const {return Rot3::Logmap(deltaRij_, H);} // super-expensive const double& deltaTij() const{return deltaTij_;} const Matrix3& delRdelBiasOmega() const{ return delRdelBiasOmega_;} @@ -99,17 +99,18 @@ public: boost::optional H) const { // First, we correct deltaRij using the biasOmegaIncr, rotated const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); - // This was done via an expmap, now we go *back* to so<3> - const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + if (H) { - // We then do a very expensive Jacobian calculation. TODO Right Duy ? - const Matrix3 Jrinv_theta_bc = // - Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); + 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 Matrix3 Jr_JbiasOmegaIncr = // Rot3::rightJacobianExpMapSO3(delRdelBiasOmega_ * biasOmegaIncr); (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; + return theta_biascorrected; } - return theta_biascorrected; + //else + return Rot3::Logmap(deltaRij_biascorrected); } /// Integrate coriolis correction in body frame rot_i diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index be3f9afa5..f91a889f2 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -239,18 +239,24 @@ public: const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis); Vector3 theta_corrected = theta_biascorrected - coriolis; - // Prediction - const Rot3 deltaRij_corrected = Rot3::Expmap( theta_corrected ); - - // Residual rotation error - const Rot3 fRhat = deltaRij_corrected.between(Ri.between(Rj)); + Rot3 deltaRij_corrected, fRhat; + Vector3 fR; // Accessory matrix, used to build the jacobians Matrix3 Jr_theta_bcc, Jtheta, Jrinv_fRhat; - if(H1 || H3 || H5){ - Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); - Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_corrected); + + // 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); + // Residual rotation error + fRhat = deltaRij_corrected.between(Ri.between(Rj)); + fR = Rot3::Logmap(fRhat, Jrinv_fRhat); Jtheta = -Jr_theta_bcc * skewSymmetric(coriolis); + }else{ + deltaRij_corrected = Rot3::Expmap( theta_corrected); + // Residual rotation error + fRhat = deltaRij_corrected.between(Ri.between(Rj)); + fR = Rot3::Logmap(fRhat); } if(H1) { @@ -339,8 +345,7 @@ public: const Vector3 fv = vel_j - predictedState_j.velocity; - // This is the same as: dR = (predictedState_j.pose.translation()).between(Rot_j) - const Vector3 fR = Rot3::Logmap(fRhat); + // fR was computes earlier. Note: it is the same as: dR = (predictedState_j.pose.translation()).between(Rot_j) Vector r(9); r << fp, fv, fR; return r;