diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index e9df7a060..592c87bcc 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -206,21 +206,17 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_ boost::optional H3, boost::optional H4, boost::optional H5, boost::optional H6) const { - // Bias evolution model: random walk - const Vector3 fbiasAcc = bias_j.accelerometer() - bias_i.accelerometer(); - const Vector3 fbiasOmega = bias_j.gyroscope() - bias_i.gyroscope(); - - // error wrt preintegrated position, velocity, rotation - Vector r_pvR(9); - // if we need the jacobians if(H1 || H2 || H3 || H4 || H5 || H6){ - Matrix H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR; // pvR = mnemonic: position (p), velocity (v), rotation (R) + Matrix H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR, Hbias_i, Hbias_j; // pvR = mnemonic: position (p), velocity (v), rotation (R) - // include errors wrt preintegrated measurements - r_pvR << ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i, + // error wrt preintegrated measurements + Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i, H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR); + // error wrt bias evolution model (random walk) + Vector6 fbias = bias_j.between(bias_i, Hbias_j, Hbias_i).vector(); // [bias_j.acc - bias_i.acc; bias_j.gyr - bias_i.gyr] + if(H1) { H1->resize(15,6); H1->block<9,6>(0,0) = H1_pvR; @@ -249,22 +245,25 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_ H5->resize(15,6); H5->block<9,6>(0,0) = H5_pvR; // adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i] - H5->block<6,6>(0,9) = - Matrix::Identity(6,6); + H5->block<6,6>(0,9) = Hbias_i; } if(H6) { H6->resize(15,6); H6->block<9,6>(0,0) = Matrix::Zero(6,6); // adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j] - H6->block<6,6>(0,9) = Matrix::Identity(6,6); + H6->block<6,6>(0,9) = Hbias_j; } - Vector r(15); r << r_pvR, fbiasAcc, fbiasOmega; // vector of size 15 + Vector r(15); r << r_pvR, fbias; // vector of size 15 return r; } // else, only compute the error vector: - // Evaluate residual error, including the model for bias evolution - r_pvR << ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i, + // error wrt preintegrated measurements + Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i, boost::none, boost::none, boost::none, boost::none, boost::none); - Vector r(15); r << r_pvR, fbiasAcc, fbiasOmega; // vector of size 15 + // error wrt bias evolution model (random walk) + Vector6 fbias = bias_j.between(bias_i).vector(); // [bias_j.acc - bias_i.acc; bias_j.gyr - bias_i.gyr] + // overall error + Vector r(15); r << r_pvR, fbias; // vector of size 15 return r; } diff --git a/gtsam/navigation/ImuFactorBase.h b/gtsam/navigation/ImuFactorBase.h index 36fa3616d..30c0f8c0e 100644 --- a/gtsam/navigation/ImuFactorBase.h +++ b/gtsam/navigation/ImuFactorBase.h @@ -52,15 +52,24 @@ protected: public: + /** Default constructor - only use for serialization */ ImuFactorBase() : gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)), body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {} + /** + * Default constructor, stores basic quantities required by the Imu factors + * @param gravity Gravity vector expressed in the global frame + * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame + * @param body_P_sensor Optional pose of the sensor frame in the body frame + * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect + */ ImuFactorBase(const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) : gravity_(gravity), omegaCoriolis_(omegaCoriolis), body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {} + /// Methods to access class variables const Vector3& gravity() const { return gravity_; } const Vector3& omegaCoriolis() const { return omegaCoriolis_; } @@ -91,6 +100,7 @@ public: boost::optional H3, boost::optional H4, boost::optional H5) const{ const double& deltaTij = preintegratedMeasurements_.deltaTij_; + // We need the mistmatch w.r.t. the biases used for preintegration const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); @@ -100,7 +110,7 @@ public: const Vector3 pos_i = pose_i.translation().vector(); const Vector3 pos_j = pose_j.translation().vector(); - // We compute factor's Jacobians + // Jacobian computation /* ---------------------------------------------------------------------------------------------------- */ const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) @@ -114,11 +124,8 @@ public: 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) { @@ -134,7 +141,6 @@ public: dfPdPi = - Rot_i.matrix(); dfVdPi = Z_3x3; } - (*H1) << // dfP/dRi Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ @@ -151,7 +157,6 @@ public: // dfR/dPi Z_3x3; } - if(H2) { H2->resize(9,3); (*H2) << @@ -164,7 +169,6 @@ public: // dfR/dVi Z_3x3; } - if(H3) { H3->resize(9,6); (*H3) << @@ -175,7 +179,6 @@ public: // dfR/dPosej Jrinv_fRhat * ( I_3x3 ), Z_3x3; } - if(H4) { H4->resize(9,3); (*H4) << @@ -186,7 +189,6 @@ public: // dfR/dVj Z_3x3; } - if(H5) { const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);