diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 592c87bcc..b3e8d4b11 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -91,11 +91,13 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements /* ----------------------------------------------------------------------------------------------------------------------- */ - const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) + const Vector3 theta_i = thetaRij(); // super-expensive parametrization of so(3) const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); - Rot3 Rot_j = deltaRij_ * Rincr; - const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) + // Update preintegrated measurements. TODO Frank moved from end of this function !!! + updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT); + + const Vector3 theta_j = thetaRij(); // super-expensive parametrization of so(3) const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); // Single Jacobians to propagate covariance @@ -105,10 +107,10 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( Matrix3 H_vel_pos = Z_3x3; Matrix3 H_vel_vel = I_3x3; - Matrix3 H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; + Matrix3 H_vel_angles = - deltaRij() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; // analytic expression corresponding to the following numerical derivative // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); - Matrix3 H_vel_biasacc = - deltaRij_.matrix() * deltaT; + Matrix3 H_vel_biasacc = - deltaRij() * deltaT; Matrix3 H_angles_pos = Z_3x3; Matrix3 H_angles_vel = Z_3x3; @@ -150,10 +152,6 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( G_measCov_Gt.block<3,3>(6,3) = block23.transpose(); preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; - - // Update preintegrated measurements - /* ----------------------------------------------------------------------------------------------------------------------- */ - updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT); } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 29c85de21..640e02641 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -86,11 +86,14 @@ 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 /* ----------------------------------------------------------------------------------------------------------------------- */ - const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) + const Vector3 theta_i = thetaRij(); // super-expensive parametrization of so(3) const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); - Rot3 Rot_j = deltaRij_ * Rincr; - const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) + // Update preintegrated measurements. TODO Frank moved from end of this function !!! + // Even though Luca says has to happen after ? Don't understand why. + updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT); + + const Vector3 theta_j = thetaRij(); // super-expensive parametrization of so(3) const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); Matrix H_pos_pos = I_3x3; @@ -99,7 +102,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( Matrix H_vel_pos = Z_3x3; Matrix H_vel_vel = I_3x3; - Matrix H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; + Matrix H_vel_angles = - deltaRij() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; // analytic expression corresponding to the following numerical derivative // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); @@ -130,10 +133,6 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT; // // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose(); - - // Update preintegrated measurements (this has to be done after the update of covariances and jacobians!) - /* ----------------------------------------------------------------------------------------------------------------------- */ - updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT); } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/ImuFactorBase.h b/gtsam/navigation/ImuFactorBase.h index c06331eeb..7dbe6c9ec 100644 --- a/gtsam/navigation/ImuFactorBase.h +++ b/gtsam/navigation/ImuFactorBase.h @@ -99,7 +99,7 @@ public: 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 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(); @@ -111,10 +111,11 @@ public: // Jacobian computation /* ---------------------------------------------------------------------------------------------------- */ - 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); + // 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 = + preintegratedMeasurements_.biascorrectedThetaRij(biasOmegaIncr, H5); Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term @@ -122,6 +123,7 @@ public: 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); @@ -189,9 +191,8 @@ public: 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 by this point already contains 3*3 biascorrectedThetaRij derivative + const Matrix3 JbiasOmega = Jr_theta_bcc * (*H5); H5->resize(9,6); (*H5) << // dfP/dBias @@ -228,7 +229,7 @@ public: const PreintegrationBase& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){ - const double& deltaTij = preintegratedMeasurements.deltaTij_; + const double& deltaTij = preintegratedMeasurements.deltaTij(); const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); @@ -255,8 +256,10 @@ public: vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity } - const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); + const Rot3 deltaRij_biascorrected = preintegratedMeasurements.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 diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 3ce3336df..173228c61 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -21,19 +21,18 @@ #pragma once -/* GTSAM includes */ +#include #include namespace gtsam { - /** * PreintegrationBase is the base class for PreintegratedMeasurements * (in ImuFactor) and CombinedPreintegratedMeasurements (in CombinedImuFactor). * It includes the definitions of the preintegrated variables and the methods * to access, print, and compare them. */ -class PreintegrationBase { +class PreintegrationBase : public PreintegratedRotation { friend class ImuFactorBase; friend class ImuFactor; @@ -45,14 +44,11 @@ protected: Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) - double deltaTij_; ///< Time interval from i to j Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias - Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias public: @@ -65,82 +61,90 @@ public: PreintegrationBase(const imuBias::ConstantBias& bias, const bool use2ndOrderIntegration) : biasHat_(bias), use2ndOrderIntegration_(use2ndOrderIntegration), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), - deltaRij_(Rot3()), deltaTij_(0.0), delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), - delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3), - delRdelBiasOmega_(Z_3x3) {} + delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3) {} + + /// methods to access class variables + const Vector3& deltaPij() const {return deltaPij_;} + const Vector3& deltaVij() const {return deltaVij_;} + Vector biasHat() const { return biasHat_.vector();} // TODO expensive + const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_;} + const Matrix3& delPdelBiasOmega() const { return delPdelBiasOmega_;} + const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_;} + const Matrix3& delVdelBiasOmega() const { return delVdelBiasOmega_;} /// Needed for testable void print(const std::string& s) const { - std::cout << s << std::endl; - biasHat_.print(" biasHat"); - std::cout << " deltaTij " << deltaTij_ << std::endl; + PreintegratedRotation::print(s); std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; - deltaRij_.print(" deltaRij "); + biasHat_.print(" biasHat"); } /// Needed for testable bool equals(const PreintegrationBase& expected, double tol) const { - return biasHat_.equals(expected.biasHat_, tol) + return PreintegratedRotation::equals(expected, tol) + && biasHat_.equals(expected.biasHat_, tol) && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) - && deltaRij_.equals(expected.deltaRij_, tol) - && fabs(deltaTij_ - expected.deltaTij_) < tol && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) - && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) - && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); + && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol); } /// Re-initialize PreintegratedMeasurements void resetIntegration(){ + PreintegratedRotation::resetIntegration(); deltaPij_ = Vector3::Zero(); deltaVij_ = Vector3::Zero(); - deltaRij_ = Rot3(); - deltaTij_ = 0.0; delPdelBiasAcc_ = Z_3x3; delPdelBiasOmega_ = Z_3x3; delVdelBiasAcc_ = Z_3x3; delVdelBiasOmega_ = Z_3x3; - delRdelBiasOmega_ = Z_3x3; } /// Update preintegrated measurements - void updatePreintegratedMeasurements(const Vector3& correctedAcc, const Rot3& Rincr, double deltaT){ + void updatePreintegratedMeasurements(const Vector3& correctedAcc, + const Rot3& incrR, double deltaT) { + Matrix3 dRij = deltaRij(); // expensive + Vector3 temp = dRij * correctedAcc * deltaT; if(!use2ndOrderIntegration_){ deltaPij_ += deltaVij_ * deltaT; }else{ - deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * correctedAcc * deltaT*deltaT; + deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT; } - deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT; - deltaRij_ = deltaRij_ * Rincr; - deltaTij_ += deltaT; + deltaVij_ += temp; + // TODO: we update rotation *after* the others. Is that correct? + updateIntegratedRotationAndDeltaT(incrR,deltaT); } /// Update Jacobians to be used during preintegration void updatePreintegratedJacobians(const Vector3& correctedAcc, - const Matrix3& Jr_theta_incr, const Rot3& Rincr, double deltaT){ - if(!use2ndOrderIntegration_){ + const Matrix3& Jr_theta_incr, const Rot3& incrR, double deltaT){ + Matrix3 dRij = deltaRij(); // expensive + Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega(); + if (!use2ndOrderIntegration_) { delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; - }else{ - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix() - * skewSymmetric(correctedAcc) * deltaT*deltaT * delRdelBiasOmega_; + } else { + delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT; + delPdelBiasOmega_ += deltaT*(delVdelBiasOmega_ + temp * 0.5); } - delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT; - delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_; - delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT; + delVdelBiasAcc_ += -dRij * deltaT; + delVdelBiasOmega_ += temp; + // TODO: we update rotation *after* the others. Is that correct? + update_delRdelBiasOmega(Jr_theta_incr,incrR,deltaT); } - void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, const Vector3& measuredOmega, - Vector3& correctedAcc, Vector3& correctedOmega, boost::optional body_P_sensor){ + void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, + const Vector3& measuredOmega, Vector3& correctedAcc, + Vector3& correctedOmega, boost::optional body_P_sensor) { correctedAcc = biasHat_.correctAccelerometer(measuredAcc); correctedOmega = biasHat_.correctGyroscope(measuredOmega); - // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame + // Then compensate for sensor-body displacement: we express the quantities + // (originally in the IMU frame) into the body frame if(body_P_sensor){ Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame @@ -150,18 +154,6 @@ public: } } - /// methods to access class variables - Matrix deltaRij() const {return deltaRij_.matrix();} - double deltaTij() const{return deltaTij_;} - Vector deltaPij() const {return deltaPij_;} - Vector deltaVij() const {return deltaVij_;} - Vector biasHat() const { return biasHat_.vector();} - Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;} - Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;} - Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;} - Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;} - Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;} - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ // 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, @@ -189,16 +181,14 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation); ar & BOOST_SERIALIZATION_NVP(biasHat_); ar & BOOST_SERIALIZATION_NVP(deltaPij_); ar & BOOST_SERIALIZATION_NVP(deltaVij_); - ar & BOOST_SERIALIZATION_NVP(deltaRij_); - ar & BOOST_SERIALIZATION_NVP(deltaTij_); ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); - ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); } };