diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 430f9f4a4..ddd066489 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -182,149 +182,11 @@ bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const { //------------------------------------------------------------------------------ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias, + const imuBias::ConstantBias& bias_i, boost::optional H1, boost::optional H2, boost::optional H3, boost::optional H4, - boost::optional H5) const -{ - - const double& deltaTij = preintegratedMeasurements_.deltaTij_; - const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); - - // we give some shorter name to rotations and translations - const Rot3 Rot_i = pose_i.rotation(); - const Rot3 Rot_j = pose_j.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - const Vector3 pos_j = pose_j.translation().vector(); - - // We compute factor's Jacobians - /* ---------------------------------------------------------------------------------------------------- */ - const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term - - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - - const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(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)); - - if(H1) { - H1->resize(9,6); - - Matrix3 dfPdPi; - Matrix3 dfVdPi; - if(use2ndOrderCoriolis_){ - dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; - dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; - } - else{ - dfPdPi = - Rot_i.matrix(); - dfVdPi = Z_3x3; - } - - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr), - // dfP/dPi - dfPdPi, - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr), - // dfV/dPi - dfVdPi, - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Z_3x3; - } - - if(H2) { - H2->resize(9,3); - (*H2) << - // dfP/dVi - - I_3x3 * deltaTij - + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper - // dfV/dVi - - I_3x3 - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Z_3x3; - } - - if(H3) { - H3->resize(9,6); - (*H3) << - // dfP/dPosej - Z_3x3, Rot_j.matrix(), - // dfV/dPosej - Matrix::Zero(3,6), - // dfR/dPosej - Jrinv_fRhat * ( I_3x3 ), Z_3x3; - } - - if(H4) { - H4->resize(9,3); - (*H4) << - // dfP/dVj - Z_3x3, - // dfV/dVj - I_3x3, - // dfR/dVj - Z_3x3; - } - - if(H5) { - const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); - const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; - - H5->resize(9,6); - (*H5) << - // dfP/dBias - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_, - // dfV/dBias - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_, - // dfR/dBias - Matrix::Zero(3,3), - Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); - } - - // Evaluate residual error, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 fp = - pos_j - pos_i - - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr) - - vel_i * deltaTij - + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - - 0.5 * gravity_ * deltaTij*deltaTij; - - const Vector3 fv = - vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr) - + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term - - gravity_ * deltaTij; - - const Vector3 fR = Rot3::Logmap(fRhat); - - Vector r(9); r << fp, fv, fR; - return r; + boost::optional H5) const{ + return ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i, H1, H2, H3, H4, H5); } } /// namespace gtsam diff --git a/gtsam/navigation/ImuFactorBase.h b/gtsam/navigation/ImuFactorBase.h index a3a89d31c..66da084a1 100644 --- a/gtsam/navigation/ImuFactorBase.h +++ b/gtsam/navigation/ImuFactorBase.h @@ -23,6 +23,7 @@ /* GTSAM includes */ #include +#include namespace gtsam { @@ -83,6 +84,151 @@ public: (body_P_sensor_ && expected.body_P_sensor_ && body_P_sensor_->equals(*expected.body_P_sensor_))); } + /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j + //------------------------------------------------------------------------------ + Vector computeErrorAndJacobians(const PreintegrationBase& preintegratedMeasurements_, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, boost::optional H1, boost::optional H2, + boost::optional H3, boost::optional H4, boost::optional H5) const{ + + const double& deltaTij = preintegratedMeasurements_.deltaTij_; + const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); + const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); + + // we give some shorter name to rotations and translations + const Rot3 Rot_i = pose_i.rotation(); + const Rot3 Rot_j = pose_j.rotation(); + const Vector3 pos_i = pose_i.translation().vector(); + const Vector3 pos_j = pose_j.translation().vector(); + + // We compute factor's Jacobians + /* ---------------------------------------------------------------------------------------------------- */ + const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); + // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) + + Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + + Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - + Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term + + const Rot3 deltaRij_biascorrected_corioliscorrected = + Rot3::Expmap( theta_biascorrected_corioliscorrected ); + + const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); + + const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(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)); + + if(H1) { + H1->resize(9,6); + + Matrix3 dfPdPi; + Matrix3 dfVdPi; + if(use2ndOrderCoriolis_){ + dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; + dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; + } + else{ + dfPdPi = - Rot_i.matrix(); + dfVdPi = Z_3x3; + } + + (*H1) << + // dfP/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ + + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr), + // dfP/dPi + dfPdPi, + // dfV/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ + + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr), + // dfV/dPi + dfVdPi, + // dfR/dRi + Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), + // dfR/dPi + Z_3x3; + } + + if(H2) { + H2->resize(9,3); + (*H2) << + // dfP/dVi + - I_3x3 * deltaTij + + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper + // dfV/dVi + - I_3x3 + + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term + // dfR/dVi + Z_3x3; + } + + if(H3) { + H3->resize(9,6); + (*H3) << + // dfP/dPosej + Z_3x3, Rot_j.matrix(), + // dfV/dPosej + Matrix::Zero(3,6), + // dfR/dPosej + Jrinv_fRhat * ( I_3x3 ), Z_3x3; + } + + if(H4) { + H4->resize(9,3); + (*H4) << + // dfP/dVj + Z_3x3, + // dfV/dVj + I_3x3, + // dfR/dVj + Z_3x3; + } + + if(H5) { + const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); + const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); + const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; + + H5->resize(9,6); + (*H5) << + // dfP/dBias + - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_, + - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_, + // dfV/dBias + - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_, + - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_, + // dfR/dBias + Matrix::Zero(3,3), + Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); + } + + // Evaluate residual error, according to [3] + /* ---------------------------------------------------------------------------------------------------- */ + const Vector3 fp = + pos_j - pos_i + - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_ + + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr + + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr) + - vel_i * deltaTij + + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + - 0.5 * gravity_ * deltaTij*deltaTij; + + const Vector3 fv = + vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_ + + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr + + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr) + + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term + - gravity_ * deltaTij; + + const Vector3 fR = Rot3::Logmap(fRhat); + + Vector r(9); r << fp, fv, fR; + return r; + } + /// Predict state at time j //------------------------------------------------------------------------------ static PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,