diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index edd57d17f..31ea65ab1 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -209,7 +209,9 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_ Matrix H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR, Hbias_i, Hbias_j; // pvR = mnemonic: position (p), velocity (v), rotation (R) // error wrt preintegrated measurements - Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(_PIM_, pose_i, vel_i, pose_j, vel_j, bias_i, + Vector r_pvR(9); + r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, + gravity_, omegaCoriolis_, use2ndOrderCoriolis_, // H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR); // error wrt bias evolution model (random walk) @@ -256,7 +258,9 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_ } // else, only compute the error vector: // error wrt preintegrated measurements - Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(_PIM_, pose_i, vel_i, pose_j, vel_j, bias_i, + Vector r_pvR(9); + r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, + gravity_, omegaCoriolis_, use2ndOrderCoriolis_, // boost::none, boost::none, boost::none, boost::none, boost::none); // 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] diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 03a0dd824..3e84c3e67 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -212,14 +212,6 @@ public: boost::optional H5 = boost::none, boost::optional H6 = boost::none) const; - /// predicted states from IMU - static PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias_i, - const PreintegrationBase& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false){ - return ImuFactorBase::predict(pose_i, vel_i, bias_i, preintegratedMeasurements, gravity, omegaCoriolis, use2ndOrderCoriolis); - } - private: /** Serialization function */ diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 1b055bef2..07781a2b6 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -148,7 +148,8 @@ ImuFactor::ImuFactor( const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor, const bool use2ndOrderCoriolis) : - Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias), + Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), + pose_i, vel_i, pose_j, vel_j, bias), ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis), _PIM_(preintegratedMeasurements) {} @@ -180,13 +181,14 @@ 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_i, - boost::optional H1, boost::optional H2, - boost::optional H3, boost::optional H4, - boost::optional H5) const{ +Vector ImuFactor::evaluateError(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 { - return ImuFactorBase::computeErrorAndJacobians(_PIM_, pose_i, vel_i, pose_j, vel_j, bias_i, H1, H2, H3, H4, H5); + return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, + gravity_, omegaCoriolis_, use2ndOrderCoriolis_, H1, H2, H3, H4, H5); } } /// namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 01a245f8b..999fda80f 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -197,13 +197,6 @@ public: boost::optional H4 = boost::none, boost::optional H5 = boost::none) const; - /// predicted states from IMU - static PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias_i, const PreintegrationBase& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false){ - return ImuFactorBase::predict(pose_i, vel_i, bias_i, preintegratedMeasurements, gravity, omegaCoriolis, use2ndOrderCoriolis); - } - private: /** Serialization function */ diff --git a/gtsam/navigation/ImuFactorBase.h b/gtsam/navigation/ImuFactorBase.h index 2d637d117..1b7919a82 100644 --- a/gtsam/navigation/ImuFactorBase.h +++ b/gtsam/navigation/ImuFactorBase.h @@ -27,20 +27,6 @@ namespace gtsam { -/** - * Struct to hold all state variables of returned by Predict function - */ -struct PoseVelocityBias { - Pose3 pose; - Vector3 velocity; - imuBias::ConstantBias bias; - - PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, - const imuBias::ConstantBias _bias) : - pose(_pose), velocity(_velocity), bias(_bias) { - } -}; - class ImuFactorBase { protected: @@ -93,184 +79,6 @@ 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& _PIM, 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 = _PIM.deltaTij(); - // We need the mistmatch w.r.t. the biases used for preintegration - const Vector3 biasAccIncr = bias_i.accelerometer() - _PIM.biasHat().accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - _PIM.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_j = pose_j.translation().vector(); - - // Jacobian computation - /* ---------------------------------------------------------------------------------------------------- */ - // 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 = - _PIM.biascorrectedThetaRij(biasOmegaIncr, H5); - - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term - - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - - // TODO: these are not always needed - 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(_PIM.deltaPij() - + _PIM.delPdelBiasOmega() * biasOmegaIncr + _PIM.delPdelBiasAcc() * biasAccIncr), - // dfP/dPi - dfPdPi, - // dfV/dRi - Rot_i.matrix() * skewSymmetric(_PIM.deltaVij() - + _PIM.delVdelBiasOmega() * biasOmegaIncr + _PIM.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) { - // H5 by this point already contains 3*3 biascorrectedThetaRij derivative - const Matrix3 JbiasOmega = Jr_theta_bcc * (*H5); - H5->resize(9,6); - (*H5) << - // dfP/dBias - - Rot_i.matrix() * _PIM.delPdelBiasAcc(), - - Rot_i.matrix() * _PIM.delPdelBiasOmega(), - // dfV/dBias - - Rot_i.matrix() * _PIM.delVdelBiasAcc(), - - Rot_i.matrix() * _PIM.delVdelBiasOmega(), - // dfR/dBias - Matrix::Zero(3,3), - Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); - } - - // Evaluate residual error, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - PoseVelocityBias predictedState_j = ImuFactorBase::predict(pose_i, vel_i, bias_i, _PIM, - gravity_, omegaCoriolis_, use2ndOrderCoriolis_); - - const Vector3 fp = pos_j - predictedState_j.pose.translation().vector(); - - 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); - - 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, - const imuBias::ConstantBias& bias_i, - const PreintegrationBase& _PIM, - const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){ - - const double& deltaTij = _PIM.deltaTij(); - const Vector3 biasAccIncr = bias_i.accelerometer() - _PIM.biasHat().accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - _PIM.biasHat().gyroscope(); - - const Rot3& Rot_i = pose_i.rotation(); - const Vector3& pos_i = pose_i.translation().vector(); - - // Predict state at time j - /* ---------------------------------------------------------------------------------------------------- */ - Vector3 pos_j = pos_i + Rot_i.matrix() * (_PIM.deltaPij() - + _PIM.delPdelBiasAcc() * biasAccIncr - + _PIM.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; - - Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (_PIM.deltaVij() - + _PIM.delVdelBiasAcc() * biasAccIncr - + _PIM.delVdelBiasOmega() * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); - - if(use2ndOrderCoriolis){ - pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position - vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity - } - - const Rot3 deltaRij_biascorrected = _PIM.biascorrectedDeltaRij(biasOmegaIncr); - // TODO Frank says comment below does not reflect what was in code - // 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 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); - - Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); - return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant - } - }; } /// namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index fd5597aff..6cf6770c4 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -26,6 +26,20 @@ namespace gtsam { +/** + * Struct to hold all state variables of returned by Predict function + */ +struct PoseVelocityBias { + Pose3 pose; + Vector3 velocity; + imuBias::ConstantBias bias; + + PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, + const imuBias::ConstantBias _bias) : + pose(_pose), velocity(_velocity), bias(_bias) { + } +}; + /** * PreintegrationBase is the base class for PreintegratedMeasurements * (in ImuFactor) and CombinedPreintegratedMeasurements (in CombinedImuFactor). @@ -150,6 +164,185 @@ public: } } + /// Predict state at time j + //------------------------------------------------------------------------------ + PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, + const imuBias::ConstantBias& bias_i, const Vector3& gravity, + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false) const { + + const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); + const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); + + const Rot3& Rot_i = pose_i.rotation(); + const Vector3& pos_i = pose_i.translation().vector(); + + // Predict state at time j + /* ---------------------------------------------------------------------------------------------------- */ + Vector3 pos_j = pos_i + Rot_i.matrix() * (deltaPij() + + delPdelBiasAcc() * biasAccIncr + + 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(); + + Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (deltaVij() + + delVdelBiasAcc() * biasAccIncr + + delVdelBiasOmega() * biasOmegaIncr) + - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij() // Coriolis term + + gravity * deltaTij()); + + if(use2ndOrderCoriolis){ + pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij()*deltaTij(); // 2nd order coriolis term for position + vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij(); // 2nd order term for velocity + } + + const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); + // TODO Frank says comment below does not reflect what was in code + // 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 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); + + Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); + return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant + } + + /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j + //------------------------------------------------------------------------------ + Vector computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, + const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, const Vector3& gravity, + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis, + boost::optional H1, boost::optional H2, + boost::optional H3, boost::optional H4, + boost::optional H5) const { + + // We need the mistmatch w.r.t. the biases used for preintegration + const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); + const Vector3 biasOmegaIncr = bias_i.gyroscope() - 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_j = pose_j.translation().vector(); + + // Jacobian computation + /* ---------------------------------------------------------------------------------------------------- */ + // 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 theta_biascorrected_corioliscorrected = theta_biascorrected - + Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term + + const Rot3 deltaRij_biascorrected_corioliscorrected = + Rot3::Expmap( theta_biascorrected_corioliscorrected ); + + // TODO: these are not always needed + 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(deltaPij() + + delPdelBiasOmega() * biasOmegaIncr + delPdelBiasAcc() * biasAccIncr), + // dfP/dPi + dfPdPi, + // dfV/dRi + Rot_i.matrix() * skewSymmetric(deltaVij() + + delVdelBiasOmega() * biasOmegaIncr + 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) { + // H5 by this point already contains 3*3 biascorrectedThetaRij derivative + const Matrix3 JbiasOmega = Jr_theta_bcc * (*H5); + H5->resize(9,6); + (*H5) << + // dfP/dBias + - Rot_i.matrix() * delPdelBiasAcc(), + - Rot_i.matrix() * delPdelBiasOmega(), + // dfV/dBias + - Rot_i.matrix() * delVdelBiasAcc(), + - Rot_i.matrix() * delVdelBiasOmega(), + // dfR/dBias + Matrix::Zero(3,3), + Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); + } + + // Evaluate residual error, according to [3] + /* ---------------------------------------------------------------------------------------------------- */ + PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, + omegaCoriolis, use2ndOrderCoriolis); + + const Vector3 fp = pos_j - predictedState_j.pose.translation().vector(); + + 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); + + Vector r(9); r << fp, fv, fR; + return r; + } + /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 656fc1c6f..b55396041 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -290,7 +290,7 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity){ // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocityBias = Combinedfactor.predict(x1, v1, bias, Combined_pre_int_data, gravity, omegaCoriolis); + PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); Vector3 expectedVelocity; expectedVelocity<<0,1,0; EXPECT(assert_equal(expectedPose, poseVelocityBias.pose)); @@ -319,7 +319,7 @@ TEST(CombinedImuFactor, PredictRotation) { // Predict Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0)); Vector3 v(0,0,0); - PoseVelocityBias poseVelocityBias = Combinedfactor.predict(x,v,bias, Combined_pre_int_data, gravity, omegaCoriolis); + PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x,v,bias, gravity, omegaCoriolis); Pose3 expectedPose(Rot3().ypr(M_PI/10, 0,0), Point3(0,0,0)); EXPECT(assert_equal(expectedPose, poseVelocityBias.pose, tol)); } diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index f9faa8b99..57becd86c 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -561,7 +561,7 @@ TEST(ImuFactor, PredictPositionAndVelocity){ // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocity = factor.predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis); + PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); Vector3 expectedVelocity; expectedVelocity<<0,1,0; EXPECT(assert_equal(expectedPose, poseVelocity.pose)); @@ -593,7 +593,7 @@ TEST(ImuFactor, PredictRotation) { // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocity = factor.predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis); + PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); Pose3 expectedPose(Rot3().ypr(M_PI/10, 0, 0), Point3(0, 0, 0)); Vector3 expectedVelocity; expectedVelocity<<0,0,0; EXPECT(assert_equal(expectedPose, poseVelocity.pose));