diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 01de5a8f3..17c68139c 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -69,25 +69,27 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); - const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement - Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr - const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement + // rotation increment computed from the current rotation rate measurement + const Vector3 integratedOmega = correctedOmega * deltaT; + Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr + // rotation increment computed from the current rotation rate measurement + const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // Update Jacobians updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT); // Update preintegrated measurements (also get Jacobian) - const Matrix3 R_i = deltaRij(); // store this, which is useful to compute G_test - Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx) + const Matrix3 R_i = deltaRij(); // store this, which is useful to compute G_test + Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx) updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F); // first order covariance propagation: - // as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework - /* ----------------------------------------------------------------------------------------------------------------------- */ - // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose(); + // as in [2] we consider a first order propagation that can be seen as a prediction phase in EKF + /* --------------------------------------------------------------------------------------------*/ + // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G' // NOTE 1: (1/deltaT) allows to pass from continuous time noise to discrete time noise // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT) - // NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done block-wise, + // NOTE 2: computation of G * (1/deltaT) * measurementCovariance * G.transpose() done block-wise, // as G and measurementCovariance are block-diagonal matrices preintMeasCov_ = F * preintMeasCov_ * F.transpose(); preintMeasCov_.block<3, 3>(0, 0) += integrationCovariance() * deltaT; @@ -101,23 +103,24 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( F_pos_noiseacc = 0.5 * R_i * deltaT * deltaT; preintMeasCov_.block<3, 3>(0, 0) += (1 / deltaT) * F_pos_noiseacc * accelerometerCovariance() * F_pos_noiseacc.transpose(); - Matrix3 temp = F_pos_noiseacc * accelerometerCovariance() * R_i.transpose(); // already includes 1/deltaT + Matrix3 temp = F_pos_noiseacc * accelerometerCovariance() * R_i.transpose(); // has 1/deltaT preintMeasCov_.block<3, 3>(0, 3) += temp; preintMeasCov_.block<3, 3>(3, 0) += temp.transpose(); } // F_test and G_test are given as output for testing purposes and are not needed by the factor - if (F_test) { // This in only for testing + if (F_test) { (*F_test) << F; } - if (G_test) { // This in only for testing & documentation, while the actual computation is done block-wise + if (G_test) { + // This in only for testing & documentation, while the actual computation is done block-wise if (!use2ndOrderIntegration()) F_pos_noiseacc = Z_3x3; // intNoise accNoise omegaNoise - (*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos - Z_3x3, R_i * deltaT, Z_3x3, // vel - Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle + (*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos + Z_3x3, R_i * deltaT, Z_3x3, // vel + Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle } } @@ -176,4 +179,4 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, gravity_, omegaCoriolis_, use2ndOrderCoriolis_, H1, H2, H3, H4, H5); } -} /// namespace gtsam +} // namespace gtsam diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 2379f58af..ba10fd090 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -31,31 +31,40 @@ namespace gtsam { * It includes the definitions of the preintegrated rotation. */ class PreintegratedRotation { - - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) - double deltaTij_; ///< Time interval from i to j + Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) + double deltaTij_; ///< Time interval from i to j /// Jacobian of preintegrated rotation w.r.t. angular rate bias Matrix3 delRdelBiasOmega_; - Matrix3 gyroscopeCovariance_; ///< continuous-time "Covariance" of gyroscope measurements -public: + ///< continuous-time "Covariance" of gyroscope measurements + const Matrix3 gyroscopeCovariance_; - /** - * Default constructor, initializes the variables in the base class - */ - PreintegratedRotation(const Matrix3& measuredOmegaCovariance) : + public: + /// Default constructor, initializes the variables in the base class + explicit PreintegratedRotation(const Matrix3& measuredOmegaCovariance) : deltaRij_(Rot3()), deltaTij_(0.0), delRdelBiasOmega_(Z_3x3), gyroscopeCovariance_( measuredOmegaCovariance) { } - /// methods to access class variables + /// Re-initialize PreintegratedMeasurements + void resetIntegration() { + deltaRij_ = Rot3(); + deltaTij_ = 0.0; + delRdelBiasOmega_ = Z_3x3; + } + + /// @name Access instance variables + /// @{ + + // Return 3*3 matrix of rotation from time i to time j. Expensive in quaternion case Matrix3 deltaRij() const { return deltaRij_.matrix(); - } // expensive + } + // Return log(rotation) from time i to time j. Expensive in both Rot3M and quaternion case. Vector3 thetaRij(OptionalJacobian<3, 3> H = boost::none) const { return Rot3::Logmap(deltaRij_, H); - } // super-expensive + } const double& deltaTij() const { return deltaTij_; } @@ -65,15 +74,17 @@ public: const Matrix3& gyroscopeCovariance() const { return gyroscopeCovariance_; } + /// @} + + /// @name Testable + /// @{ - /// Needed for testable void print(const std::string& s) const { std::cout << s << std::endl; std::cout << " deltaTij [" << deltaTij_ << "]" << std::endl; std::cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << std::endl; } - /// Needed for testable bool equals(const PreintegratedRotation& expected, double tol) const { return deltaRij_.equals(expected.deltaRij_, tol) && fabs(deltaTij_ - expected.deltaTij_) < tol @@ -83,12 +94,7 @@ public: expected.gyroscopeCovariance_, tol); } - /// Re-initialize PreintegratedMeasurements - void resetIntegration() { - deltaRij_ = Rot3(); - deltaTij_ = 0.0; - delRdelBiasOmega_ = Z_3x3; - } + /// @} /// Update preintegrated measurements void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT, @@ -104,8 +110,7 @@ public: void update_delRdelBiasOmega(const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT) { const Matrix3 incrRt = incrR.transpose(); - delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - - D_Rincr_integratedOmega * deltaT; + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_Rincr_integratedOmega * deltaT; } /// Return a bias corrected version of the integrated rotation - expensive @@ -125,12 +130,11 @@ public: // This was done via an expmap, now we go *back* to so<3> const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, Jrinv_theta_bc); - const Matrix3 Jr_JbiasOmegaIncr = // - Rot3::ExpmapDerivative(delRdelBiasOmega_ * biasOmegaIncr); + const Matrix3 Jr_JbiasOmegaIncr = Rot3::ExpmapDerivative(delRdelBiasOmega_ * biasOmegaIncr); (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; return biascorrectedOmega; } - //else + // else return Rot3::Logmap(deltaRij_biascorrected); } @@ -140,15 +144,15 @@ public: return rot_i.transpose() * omegaCoriolis * deltaTij(); } -private: + private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { // NOLINT ar & BOOST_SERIALIZATION_NVP(deltaRij_); ar & BOOST_SERIALIZATION_NVP(deltaTij_); ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); } }; -} /// namespace gtsam +} // namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 496569599..be604e784 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -41,6 +41,17 @@ PreintegrationBase::PreintegrationBase(const imuBias::ConstantBias& bias, integrationCovariance_(integrationErrorCovariance) { } +/// Re-initialize PreintegratedMeasurements +void PreintegrationBase::resetIntegration() { + PreintegratedRotation::resetIntegration(); + deltaPij_ = Vector3::Zero(); + deltaVij_ = Vector3::Zero(); + delPdelBiasAcc_ = Z_3x3; + delPdelBiasOmega_ = Z_3x3; + delVdelBiasAcc_ = Z_3x3; + delVdelBiasOmega_ = Z_3x3; +} + /// Needed for testable void PreintegrationBase::print(const std::string& s) const { PreintegratedRotation::print(s); @@ -62,17 +73,6 @@ bool PreintegrationBase::equals(const PreintegrationBase& other, double tol) con && equal_with_abs_tol(integrationCovariance_, other.integrationCovariance_, tol); } -/// Re-initialize PreintegratedMeasurements -void PreintegrationBase::resetIntegration() { - PreintegratedRotation::resetIntegration(); - deltaPij_ = Vector3::Zero(); - deltaVij_ = Vector3::Zero(); - delPdelBiasAcc_ = Z_3x3; - delPdelBiasOmega_ = Z_3x3; - delVdelBiasAcc_ = Z_3x3; - delVdelBiasOmega_ = Z_3x3; -} - /// Update preintegrated measurements void PreintegrationBase::updatePreintegratedMeasurements(const Vector3& correctedAcc, const Rot3& incrR, const double deltaT, @@ -161,7 +161,7 @@ PoseVelocityBias PreintegrationBase::predict( const Vector3 pos_i = pose_i.translation().vector(); // Predict state at time j - /* ---------------------------------------------------------------------------------------------------- */ + /* ---------------------------------------------------------------------------------------------*/ const Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr + delPdelBiasOmega() * biasOmegaIncr; if (deltaPij_biascorrected_out) // if desired, store this @@ -177,13 +177,13 @@ PoseVelocityBias PreintegrationBase::predict( if (deltaVij_biascorrected_out) // if desired, store this *deltaVij_biascorrected_out = deltaVij_biascorrected; - Vector3 vel_j = Vector3( - vel_i + Rot_i_matrix * deltaVij_biascorrected - 2 * omegaCoriolis.cross(vel_i) * dt // Coriolis term - + gravity * dt); + Vector3 vel_j = Vector3(vel_i + Rot_i_matrix * deltaVij_biascorrected - + 2 * omegaCoriolis.cross(vel_i) * dt // Coriolis term + + gravity * dt); if (use2ndOrderCoriolis) { - pos_j += -0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt2; // 2nd order coriolis term for position - vel_j += -omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt; // 2nd order term for velocity + pos_j += -0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt2; // for position + vel_j += -omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt; // for velocity } const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index f6b24e752..85ffae8a2 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -52,8 +52,8 @@ struct PoseVelocityBias { */ class PreintegrationBase : public PreintegratedRotation { - imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration - bool use2ndOrderIntegration_; ///< Controls the order of integration + const imuBias::ConstantBias biasHat_; ///< Acceleration and gyro bias used for preintegration + const bool use2ndOrderIntegration_; ///< Controls the order of integration 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) @@ -79,6 +79,9 @@ class PreintegrationBase : public PreintegratedRotation { const Matrix3& measuredOmegaCovariance, const Matrix3&integrationErrorCovariance, const bool use2ndOrderIntegration); + /// Re-initialize PreintegratedMeasurements + void resetIntegration(); + /// methods to access class variables bool use2ndOrderIntegration() const { return use2ndOrderIntegration_; @@ -121,9 +124,6 @@ class PreintegrationBase : public PreintegratedRotation { /// check equality bool equals(const PreintegrationBase& other, double tol) const; - /// Re-initialize PreintegratedMeasurements - void resetIntegration(); - /// Update preintegrated measurements void updatePreintegratedMeasurements(const Vector3& correctedAcc, const Rot3& incrR, const double deltaT, OptionalJacobian<9, 9> F);