diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 6981dee9c..ea39368d6 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -91,7 +91,7 @@ void PreintegratedImuMeasurements::integrateMeasurements(const Matrix& measuredA } //------------------------------------------------------------------------------ -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION +#ifdef GTSAM_TANGENT_PREINTEGRATION void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, // Matrix9* H1, Matrix9* H2) { PreintegrationBase::mergeWith(pim12, H1, H2); @@ -175,7 +175,7 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, } //------------------------------------------------------------------------------ -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION +#ifdef GTSAM_TANGENT_PREINTEGRATION PreintegratedImuMeasurements ImuFactor::Merge( const PreintegratedImuMeasurements& pim01, const PreintegratedImuMeasurements& pim12) { diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 6f72ec9be..f3bfd8c83 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -124,7 +124,7 @@ public: /// Return pre-integrated measurement covariance Matrix preintMeasCov() const { return preintMeasCov_; } -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION +#ifdef GTSAM_TANGENT_PREINTEGRATION /// Merge in a different set of measurements and update bias derivatives accordingly void mergeWith(const PreintegratedImuMeasurements& pim, Matrix9* H1, Matrix9* H2); #endif @@ -232,7 +232,7 @@ public: boost::optional H3 = boost::none, boost::optional H4 = boost::none, boost::optional H5 = boost::none) const; -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION +#ifdef GTSAM_TANGENT_PREINTEGRATION /// Merge two pre-integrated measurement classes static PreintegratedImuMeasurements Merge( const PreintegratedImuMeasurements& pim01, diff --git a/gtsam/navigation/ManifoldPreintegration.cpp b/gtsam/navigation/ManifoldPreintegration.cpp new file mode 100644 index 000000000..b9344a524 --- /dev/null +++ b/gtsam/navigation/ManifoldPreintegration.cpp @@ -0,0 +1,144 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ManifoldPreintegration.cpp + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#include "ManifoldPreintegration.h" + +using namespace std; + +namespace gtsam { + +//------------------------------------------------------------------------------ +ManifoldPreintegration::ManifoldPreintegration(const boost::shared_ptr& p, + const Bias& biasHat) + : PreintegrationBase(p, biasHat) { + resetIntegration(); +} + +//------------------------------------------------------------------------------ +void ManifoldPreintegration::resetIntegration() { + deltaTij_ = 0.0; + deltaXij_ = NavState(); + delRdelBiasOmega_.setZero(); + delPdelBiasAcc_.setZero(); + delPdelBiasOmega_.setZero(); + delVdelBiasAcc_.setZero(); + delVdelBiasOmega_.setZero(); +} + +//------------------------------------------------------------------------------ +bool ManifoldPreintegration::equals(const ManifoldPreintegration& other, + double tol) const { + return p_->equals(*other.p_, tol) + && fabs(deltaTij_ - other.deltaTij_) < tol + && biasHat_.equals(other.biasHat_, tol) + && deltaXij_.equals(other.deltaXij_, tol) + && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol) + && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol) + && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol) + && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol) + && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol); +} + +//------------------------------------------------------------------------------ +void ManifoldPreintegration::integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double dt, + Matrix9* A, Matrix93* B, Matrix93* C) { + + // Correct for bias in the sensor frame + Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); + Vector3 omega = biasHat_.correctGyroscope(measuredOmega); + + // Possibly correct for sensor pose + Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; + if (p().body_P_sensor) + boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, + D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); + + // Save current rotation for updating Jacobians + const Rot3 oldRij = deltaXij_.attitude(); + + // Do update + deltaTij_ += dt; + deltaXij_ = deltaXij_.update(acc, omega, dt, A, B, C); // functional + + if (p().body_P_sensor) { + // More complicated derivatives in case of non-trivial sensor pose + *C *= D_correctedOmega_omega; + if (!p().body_P_sensor->translation().isZero()) + *C += *B* D_correctedAcc_omega; + *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last + } + + // Update Jacobians + // TODO(frank): Try same simplification as in new approach + Matrix3 D_acc_R; + oldRij.rotate(acc, D_acc_R); + const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_; + + const Vector3 integratedOmega = omega * dt; + Matrix3 D_incrR_integratedOmega; + const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! + const Matrix3 incrRt = incrR.transpose(); + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * dt; + + double dt22 = 0.5 * dt * dt; + const Matrix3 dRij = oldRij.matrix(); // expensive + delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij; + delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega; + delVdelBiasAcc_ += -dRij * dt; + delVdelBiasOmega_ += D_acc_biasOmega * dt; +} + +//------------------------------------------------------------------------------ +Vector9 ManifoldPreintegration::biasCorrectedDelta( + const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { + // Correct deltaRij, derivative is delRdelBiasOmega_ + const imuBias::ConstantBias biasIncr = bias_i - biasHat_; + Matrix3 D_correctedRij_bias; + const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope(); + const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none, + H ? &D_correctedRij_bias : 0); + if (H) + D_correctedRij_bias *= delRdelBiasOmega_; + + Vector9 xi; + Matrix3 D_dR_correctedRij; + // TODO(frank): could line below be simplified? It is equivalent to + // LogMap(deltaRij_.compose(Expmap(biasInducedOmega))) + NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0); + NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer() + + delPdelBiasOmega_ * biasIncr.gyroscope(); + NavState::dV(xi) = deltaVij() + delVdelBiasAcc_ * biasIncr.accelerometer() + + delVdelBiasOmega_ * biasIncr.gyroscope(); + + if (H) { + Matrix36 D_dR_bias, D_dP_bias, D_dV_bias; + D_dR_bias << Z_3x3, D_dR_correctedRij * D_correctedRij_bias; + D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_; + D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_; + (*H) << D_dR_bias, D_dP_bias, D_dV_bias; + } + return xi; +} + +//------------------------------------------------------------------------------ + +} // namespace gtsam diff --git a/gtsam/navigation/ManifoldPreintegration.h b/gtsam/navigation/ManifoldPreintegration.h new file mode 100644 index 000000000..60ac7cf63 --- /dev/null +++ b/gtsam/navigation/ManifoldPreintegration.h @@ -0,0 +1,126 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ManifoldPreintegration.h + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#pragma once + +#include + +namespace gtsam { + +/** + * IMU pre-integration on NavSatet manifold. + * This corresponds to the original RSS paper (with one difference: V is rotated) + */ +class ManifoldPreintegration : public PreintegrationBase { + protected: + + /** + * Pre-integrated navigation state, from frame i to frame j + * Note: relative position does not take into account velocity at time i, see deltap+, in [2] + * Note: velocity is now also in frame i, as opposed to deltaVij in [2] + */ + NavState deltaXij_; + Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias + 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 + + /// Default constructor for serialization + ManifoldPreintegration() { + resetIntegration(); + } + +public: + /// @name Constructors + /// @{ + + /** + * Constructor, initializes the variables in the base class + * @param p Parameters, typically fixed in a single application + * @param bias Current estimate of acceleration and rotation rate biases + */ + ManifoldPreintegration(const boost::shared_ptr& p, + const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()); + + /// @} + + /// @name Basic utilities + /// @{ + /// Re-initialize PreintegratedMeasurements + void resetIntegration() override; + + /// @} + + /// @name Instance variables access + /// @{ + const NavState& deltaXij() const override { return deltaXij_; } + const Rot3& deltaRij() const override { return deltaXij_.attitude(); } + Vector3 deltaPij() const override { return deltaXij_.position().vector(); } + Vector3 deltaVij() const override { return deltaXij_.velocity(); } + + /// @name Testable + /// @{ + bool equals(const ManifoldPreintegration& other, double tol) const override; + /// @} + + /// @name Main functionality + /// @{ + + /// Update preintegrated measurements and get derivatives + /// It takes measured quantities in the j frame + /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose + /// NOTE(frank): implementation is different in two versions + void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, + Matrix9* A, Matrix93* B, Matrix93* C) override; + + /// Given the estimate of the bias, return a NavState tangent vector + /// summarizing the preintegrated IMU measurements so far + /// NOTE(frank): implementation is different in two versions + Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 6> H = boost::none) const override; + + /** Dummy clone for MATLAB */ + virtual boost::shared_ptr clone() const { + return boost::shared_ptr(); + } + + /// @} + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; + ar & BOOST_SERIALIZATION_NVP(p_); + ar & BOOST_SERIALIZATION_NVP(deltaTij_); + ar & BOOST_SERIALIZATION_NVP(deltaXij_); + ar & BOOST_SERIALIZATION_NVP(biasHat_); + ar & bs::make_nvp("delRdelBiasOmega_", bs::make_array(delRdelBiasOmega_.data(), delRdelBiasOmega_.size())); + ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size())); + ar & bs::make_nvp("delPdelBiasOmega_", bs::make_array(delPdelBiasOmega_.data(), delPdelBiasOmega_.size())); + ar & bs::make_nvp("delVdelBiasAcc_", bs::make_array(delVdelBiasAcc_.data(), delVdelBiasAcc_.size())); + ar & bs::make_nvp("delVdelBiasOmega_", bs::make_array(delVdelBiasOmega_.data(), delVdelBiasOmega_.size())); + } +}; + +} /// namespace gtsam diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index ee6135ede..467c5802a 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -239,7 +239,7 @@ Matrix7 NavState::wedge(const Vector9& xi) { #define D_v_v(H) (H)->block<3,3>(6,6) //------------------------------------------------------------------------------ -#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V4) || !defined(GTSAM_IMU_MANIFOLD_INTEGRATION) +#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V4) || !defined(GTSAM_TANGENT_PREINTEGRATION) NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, OptionalJacobian<9, 3> G2) const { diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 27e083014..ed87a32d4 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -203,7 +203,7 @@ public: /// @name Dynamics /// @{ -#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V4) || !defined(GTSAM_IMU_MANIFOLD_INTEGRATION) +#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V4) || !defined(GTSAM_TANGENT_PREINTEGRATION) /// Integrate forward in time given angular velocity and acceleration in body frame /// Uses second order integration for position, returns derivatives except dt. NavState update(const Vector3& b_acceleration, const Vector3& b_omega, diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index b0931b930..146fdf8b7 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -34,31 +34,10 @@ PreintegrationBase::PreintegrationBase(const boost::shared_ptr& p, resetIntegration(); } -//------------------------------------------------------------------------------ -void PreintegrationBase::resetIntegration() { - deltaTij_ = 0.0; -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION - preintegrated_.setZero(); - preintegrated_H_biasAcc_.setZero(); - preintegrated_H_biasOmega_.setZero(); -#else - deltaXij_ = NavState(); - delRdelBiasOmega_.setZero(); - delPdelBiasAcc_.setZero(); - delPdelBiasOmega_.setZero(); - delVdelBiasAcc_.setZero(); - delVdelBiasOmega_.setZero(); -#endif -} - //------------------------------------------------------------------------------ ostream& operator<<(ostream& os, const PreintegrationBase& pim) { os << " deltaTij " << pim.deltaTij_ << endl; -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION - os << " deltaRij " << Point3(pim.theta()) << endl; -#else os << " deltaRij.ypr = (" << pim.deltaRij().ypr().transpose() << ")" << endl; -#endif os << " deltaPij " << Point3(pim.deltaPij()) << endl; os << " deltaVij " << Point3(pim.deltaVij()) << endl; os << " gyrobias " << Point3(pim.biasHat_.gyroscope()) << endl; @@ -71,26 +50,6 @@ void PreintegrationBase::print(const string& s) const { cout << s << *this << endl; } -//------------------------------------------------------------------------------ -bool PreintegrationBase::equals(const PreintegrationBase& other, - double tol) const { - return p_->equals(*other.p_, tol) - && fabs(deltaTij_ - other.deltaTij_) < tol - && biasHat_.equals(other.biasHat_, tol) -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION - && equal_with_abs_tol(preintegrated_, other.preintegrated_, tol) - && equal_with_abs_tol(preintegrated_H_biasAcc_, other.preintegrated_H_biasAcc_, tol) - && equal_with_abs_tol(preintegrated_H_biasOmega_, other.preintegrated_H_biasOmega_, tol); -#else - && deltaXij_.equals(other.deltaXij_, tol) - && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol) - && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol) - && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol) - && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol) - && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol); -#endif -} - //------------------------------------------------------------------------------ pair PreintegrationBase::correctMeasurementsBySensorPose( const Vector3& unbiasedAcc, const Vector3& unbiasedOmega, @@ -137,203 +96,6 @@ pair PreintegrationBase::correctMeasurementsBySensorPose( return make_pair(correctedAcc, correctedOmega); } -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION -//------------------------------------------------------------------------------ -// See extensive discussion in ImuFactor.lyx -Vector9 PreintegrationBase::UpdatePreintegrated( - const Vector3& a_body, const Vector3& w_body, double dt, - const Vector9& preintegrated, OptionalJacobian<9, 9> A, - OptionalJacobian<9, 3> B, OptionalJacobian<9, 3> C) { - const auto theta = preintegrated.segment<3>(0); - const auto position = preintegrated.segment<3>(3); - const auto velocity = preintegrated.segment<3>(6); - - // This functor allows for saving computation when exponential map and its - // derivatives are needed at the same location in so<3> - so3::DexpFunctor local(theta); - - // Calculate exact mean propagation - Matrix3 w_tangent_H_theta, invH; - const Vector3 w_tangent = // angular velocity mapped back to tangent space - local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0); - const SO3 R = local.expmap(); - const Vector3 a_nav = R * a_body; - const double dt22 = 0.5 * dt * dt; - - Vector9 preintegratedPlus; - preintegratedPlus << // new preintegrated vector: - theta + w_tangent* dt, // theta - position + velocity* dt + a_nav* dt22, // position - velocity + a_nav* dt; // velocity - - if (A) { - // Exact derivative of R*a with respect to theta: - const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp(); - - A->setIdentity(); - A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta - A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta... - A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity - A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta - } - if (B) { - B->block<3, 3>(0, 0) = Z_3x3; - B->block<3, 3>(3, 0) = R * dt22; - B->block<3, 3>(6, 0) = R * dt; - } - if (C) { - C->block<3, 3>(0, 0) = invH * dt; - C->block<3, 3>(3, 0) = Z_3x3; - C->block<3, 3>(6, 0) = Z_3x3; - } - - return preintegratedPlus; -} - -//------------------------------------------------------------------------------ -void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, - const double dt, Matrix9* A, - Matrix93* B, Matrix93* C) { - // Correct for bias in the sensor frame - Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); - Vector3 omega = biasHat_.correctGyroscope(measuredOmega); - - // Possibly correct for sensor pose - Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; - if (p().body_P_sensor) - boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, - D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); - - // Do update - deltaTij_ += dt; - preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C); - - if (p().body_P_sensor) { - // More complicated derivatives in case of non-trivial sensor pose - *C *= D_correctedOmega_omega; - if (!p().body_P_sensor->translation().isZero()) - *C += *B* D_correctedAcc_omega; - *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last - } - - // D_plus_abias = D_plus_preintegrated * D_preintegrated_abias + D_plus_a * D_a_abias - preintegrated_H_biasAcc_ = (*A) * preintegrated_H_biasAcc_ - (*B); - - // D_plus_wbias = D_plus_preintegrated * D_preintegrated_wbias + D_plus_w * D_w_wbias - preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C); -} - -void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, - double dt) { - // NOTE(frank): integrateMeasurement always needs to compute the derivatives, - // even when not of interest to the caller. Provide scratch space here. - Matrix9 A; - Matrix93 B, C; - integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); -} -//------------------------------------------------------------------------------ -Vector9 PreintegrationBase::biasCorrectedDelta( - const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { - // We correct for a change between bias_i and the biasHat_ used to integrate - // This is a simple linear correction with obvious derivatives - const imuBias::ConstantBias biasIncr = bias_i - biasHat_; - const Vector9 biasCorrected = - preintegrated() + preintegrated_H_biasAcc_ * biasIncr.accelerometer() + - preintegrated_H_biasOmega_ * biasIncr.gyroscope(); - - if (H) { - (*H) << preintegrated_H_biasAcc_, preintegrated_H_biasOmega_; - } - return biasCorrected; -} -#else - -//------------------------------------------------------------------------------ -void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, const double dt, - Matrix9* A, Matrix93* B, Matrix93* C) { - - // Correct for bias in the sensor frame - Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); - Vector3 omega = biasHat_.correctGyroscope(measuredOmega); - - // Possibly correct for sensor pose - Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; - if (p().body_P_sensor) - boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, - D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); - - // Save current rotation for updating Jacobians - const Rot3 oldRij = deltaXij_.attitude(); - - // Do update - deltaTij_ += dt; - deltaXij_ = deltaXij_.update(acc, omega, dt, A, B, C); // functional - - if (p().body_P_sensor) { - // More complicated derivatives in case of non-trivial sensor pose - *C *= D_correctedOmega_omega; - if (!p().body_P_sensor->translation().isZero()) - *C += *B* D_correctedAcc_omega; - *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last - } - - // Update Jacobians - // TODO(frank): Try same simplification as in new approach - Matrix3 D_acc_R; - oldRij.rotate(acc, D_acc_R); - const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_; - - const Vector3 integratedOmega = omega * dt; - Matrix3 D_incrR_integratedOmega; - const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! - const Matrix3 incrRt = incrR.transpose(); - delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * dt; - - double dt22 = 0.5 * dt * dt; - const Matrix3 dRij = oldRij.matrix(); // expensive - delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij; - delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega; - delVdelBiasAcc_ += -dRij * dt; - delVdelBiasOmega_ += D_acc_biasOmega * dt; -} - -//------------------------------------------------------------------------------ -Vector9 PreintegrationBase::biasCorrectedDelta( - const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { - // Correct deltaRij, derivative is delRdelBiasOmega_ - const imuBias::ConstantBias biasIncr = bias_i - biasHat_; - Matrix3 D_correctedRij_bias; - const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope(); - const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none, - H ? &D_correctedRij_bias : 0); - if (H) - D_correctedRij_bias *= delRdelBiasOmega_; - - Vector9 xi; - Matrix3 D_dR_correctedRij; - // TODO(frank): could line below be simplified? It is equivalent to - // LogMap(deltaRij_.compose(Expmap(biasInducedOmega))) - NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0); - NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer() - + delPdelBiasOmega_ * biasIncr.gyroscope(); - NavState::dV(xi) = deltaVij() + delVdelBiasAcc_ * biasIncr.accelerometer() - + delVdelBiasOmega_ * biasIncr.gyroscope(); - - if (H) { - Matrix36 D_dR_bias, D_dP_bias, D_dV_bias; - D_dR_bias << Z_3x3, D_dR_correctedRij * D_correctedRij_bias; - D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_; - D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_; - (*H) << D_dR_bias, D_dP_bias, D_dV_bias; - } - return xi; -} - -#endif - //------------------------------------------------------------------------------ NavState PreintegrationBase::predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1, @@ -415,96 +177,6 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, return error; } -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION -//------------------------------------------------------------------------------ -// sugar for derivative blocks -#define D_R_R(H) (H)->block<3,3>(0,0) -#define D_R_t(H) (H)->block<3,3>(0,3) -#define D_R_v(H) (H)->block<3,3>(0,6) -#define D_t_R(H) (H)->block<3,3>(3,0) -#define D_t_t(H) (H)->block<3,3>(3,3) -#define D_t_v(H) (H)->block<3,3>(3,6) -#define D_v_R(H) (H)->block<3,3>(6,0) -#define D_v_t(H) (H)->block<3,3>(6,3) -#define D_v_v(H) (H)->block<3,3>(6,6) - -//------------------------------------------------------------------------------ -Vector9 PreintegrationBase::Compose(const Vector9& zeta01, - const Vector9& zeta12, double deltaT12, - OptionalJacobian<9, 9> H1, - OptionalJacobian<9, 9> H2) { - const auto t01 = zeta01.segment<3>(0); - const auto p01 = zeta01.segment<3>(3); - const auto v01 = zeta01.segment<3>(6); - - const auto t12 = zeta12.segment<3>(0); - const auto p12 = zeta12.segment<3>(3); - const auto v12 = zeta12.segment<3>(6); - - Matrix3 R01_H_t01, R12_H_t12; - const Rot3 R01 = Rot3::Expmap(t01, R01_H_t01); - const Rot3 R12 = Rot3::Expmap(t12, R12_H_t12); - - Matrix3 R02_H_R01, R02_H_R12; // NOTE(frank): R02_H_R12 == Identity - const Rot3 R02 = R01.compose(R12, R02_H_R01, R02_H_R12); - - Matrix3 t02_H_R02; - Vector9 zeta02; - const Matrix3 R = R01.matrix(); - zeta02 << Rot3::Logmap(R02, t02_H_R02), // theta - p01 + v01 * deltaT12 + R * p12, // position - v01 + R * v12; // velocity - - if (H1) { - H1->setIdentity(); - D_R_R(H1) = t02_H_R02 * R02_H_R01 * R01_H_t01; - D_t_R(H1) = R * skewSymmetric(-p12) * R01_H_t01; - D_t_v(H1) = I_3x3 * deltaT12; - D_v_R(H1) = R * skewSymmetric(-v12) * R01_H_t01; - } - - if (H2) { - H2->setZero(); - D_R_R(H2) = t02_H_R02 * R02_H_R12 * R12_H_t12; - D_t_t(H2) = R; - D_v_v(H2) = R; - } - - return zeta02; -} - -//------------------------------------------------------------------------------ -void PreintegrationBase::mergeWith(const PreintegrationBase& pim12, Matrix9* H1, - Matrix9* H2) { - if (!matchesParamsWith(pim12)) { - throw std::domain_error("Cannot merge pre-integrated measurements with different params"); - } - - if (params()->body_P_sensor) { - throw std::domain_error("Cannot merge pre-integrated measurements with sensor pose yet"); - } - - const double t01 = deltaTij(); - const double t12 = pim12.deltaTij(); - deltaTij_ = t01 + t12; - - const Vector9 zeta01 = preintegrated(); - Vector9 zeta12 = pim12.preintegrated(); // will be modified. - - const imuBias::ConstantBias bias_incr_for_12 = biasHat() - pim12.biasHat(); - zeta12 += pim12.preintegrated_H_biasOmega_ * bias_incr_for_12.gyroscope() - + pim12.preintegrated_H_biasAcc_ * bias_incr_for_12.accelerometer(); - - preintegrated_ = PreintegrationBase::Compose(zeta01, zeta12, t12, H1, H2); - - preintegrated_H_biasAcc_ = - (*H1) * preintegrated_H_biasAcc_ + (*H2) * pim12.preintegrated_H_biasAcc_; - - preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_ + - (*H2) * pim12.preintegrated_H_biasOmega_; -} -#endif - //------------------------------------------------------------------------------ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index f5560c37b..04539e96b 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -75,37 +75,13 @@ class PreintegrationBase { /// Time interval from i to j double deltaTij_; - /** - * Preintegrated navigation state, from frame i to frame j - * Order is: theta, position, velocity - * Note: relative position does not take into account velocity at time i, see deltap+, in [2] - * Note: velocity is now also in frame i, as opposed to deltaVij in [2] - */ -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION - Vector9 preintegrated_; - Matrix93 preintegrated_H_biasAcc_; ///< Jacobian of preintegrated preintegrated w.r.t. acceleration bias - Matrix93 preintegrated_H_biasOmega_; ///< Jacobian of preintegrated preintegrated w.r.t. angular rate bias -#else - NavState deltaXij_; - Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias - 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 -#endif - /// Default constructor for serialization - PreintegrationBase() { - resetIntegration(); - } + PreintegrationBase() {} public: /// @name Constructors /// @{ - /// @name Constructors - /// @{ - /** * Constructor, initializes the variables in the base class * @param p Parameters, typically fixed in a single application @@ -119,7 +95,7 @@ public: /// @name Basic utilities /// @{ /// Re-initialize PreintegratedMeasurements - void resetIntegration(); + virtual void resetIntegration()=0; /// check parameters equality: checks whether shared pointer points to same Params object. bool matchesParamsWith(const PreintegrationBase& other) const { @@ -148,22 +124,10 @@ public: const imuBias::ConstantBias& biasHat() const { return biasHat_; } double deltaTij() const { return deltaTij_; } -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION - const Vector9& preintegrated() const { return preintegrated_; } - Vector3 theta() const { return preintegrated_.head<3>(); } - Vector3 deltaPij() const { return preintegrated_.segment<3>(3); } - Vector3 deltaVij() const { return preintegrated_.tail<3>(); } - Rot3 deltaRij() const { return Rot3::Expmap(theta()); } - NavState deltaXij() const { return NavState::Retract(preintegrated_); } - - const Matrix93& preintegrated_H_biasAcc() const { return preintegrated_H_biasAcc_; } - const Matrix93& preintegrated_H_biasOmega() const { return preintegrated_H_biasOmega_; } -#else - const NavState& deltaXij() const { return deltaXij_; } - const Rot3& deltaRij() const { return deltaXij_.attitude(); } - Vector3 deltaPij() const { return deltaXij_.position().vector(); } - Vector3 deltaVij() const { return deltaXij_.velocity(); } -#endif + virtual Vector3 deltaPij() const=0; + virtual Vector3 deltaVij() const=0; + virtual Rot3 deltaRij() const=0; + virtual NavState deltaXij() const=0; // Exposed for MATLAB Vector6 biasHatVector() const { return biasHat_.vector(); } @@ -173,7 +137,7 @@ public: /// @{ GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const PreintegrationBase& pim); void print(const std::string& s) const; - bool equals(const PreintegrationBase& other, double tol) const; + virtual bool equals(const PreintegrationBase& other, double tol) const = 0; /// @} /// @name Main functionality @@ -188,34 +152,16 @@ public: OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none, OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const; -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION - - // Update integrated vector on tangent manifold preintegrated with acceleration - // Static, functional version. - static Vector9 UpdatePreintegrated(const Vector3& a_body, - const Vector3& w_body, const double dt, - const Vector9& preintegrated, - OptionalJacobian<9, 9> A = boost::none, - OptionalJacobian<9, 3> B = boost::none, - OptionalJacobian<9, 3> C = boost::none); - - // Version without derivatives - void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt); - -#endif - /// Update preintegrated measurements and get derivatives /// It takes measured quantities in the j frame /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose - /// NOTE(frank): implementation is different in two versions - void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, - Matrix9* A, Matrix93* B, Matrix93* C); + virtual void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, + const double dt, Matrix9* A, Matrix93* B, Matrix93* C)=0; /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far - /// NOTE(frank): implementation is different in two versions - Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, - OptionalJacobian<9, 6> H = boost::none) const; + virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 6> H = boost::none) const=0; /// Predict state at time j NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, @@ -236,19 +182,6 @@ public: OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 = boost::none, OptionalJacobian<9, 6> H5 = boost::none) const; -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION - // Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives - static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12, - double deltaT12, - OptionalJacobian<9, 9> H1 = boost::none, - OptionalJacobian<9, 9> H2 = boost::none); - - /// Merge in a different set of measurements and update bias derivatives accordingly - /// The derivatives apply to the preintegrated Vector9 - void mergeWith(const PreintegrationBase& pim, Matrix9* H1, Matrix9* H2); - /// @} -#endif - /** Dummy clone for MATLAB */ virtual boost::shared_ptr clone() const { return boost::shared_ptr(); @@ -267,31 +200,6 @@ public: #endif /// @} - -private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - namespace bs = ::boost::serialization; - ar & BOOST_SERIALIZATION_NVP(p_); -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION - ar & BOOST_SERIALIZATION_NVP(biasHat_); - ar & BOOST_SERIALIZATION_NVP(deltaTij_); - ar & bs::make_nvp("preintegrated_", bs::make_array(preintegrated_.data(), preintegrated_.size())); - ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size())); - ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size())); -#else - ar & BOOST_SERIALIZATION_NVP(deltaTij_); - ar & BOOST_SERIALIZATION_NVP(deltaXij_); - ar & BOOST_SERIALIZATION_NVP(biasHat_); - ar & bs::make_nvp("delRdelBiasOmega_", bs::make_array(delRdelBiasOmega_.data(), delRdelBiasOmega_.size())); - ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size())); - ar & bs::make_nvp("delPdelBiasOmega_", bs::make_array(delPdelBiasOmega_.data(), delPdelBiasOmega_.size())); - ar & bs::make_nvp("delVdelBiasAcc_", bs::make_array(delVdelBiasAcc_.data(), delVdelBiasAcc_.size())); - ar & bs::make_nvp("delVdelBiasOmega_", bs::make_array(delVdelBiasOmega_.data(), delVdelBiasOmega_.size())); -#endif - } }; } /// namespace gtsam diff --git a/gtsam/navigation/TangentPreintegration.cpp b/gtsam/navigation/TangentPreintegration.cpp new file mode 100644 index 000000000..b7e62b528 --- /dev/null +++ b/gtsam/navigation/TangentPreintegration.cpp @@ -0,0 +1,253 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file TangentPreintegration.cpp + * @author Frank Dellaert + * @author Adam Bry + **/ + +#include "TangentPreintegration.h" +#include +#include + +using namespace std; + +namespace gtsam { + +//------------------------------------------------------------------------------ +TangentPreintegration::TangentPreintegration(const boost::shared_ptr& p, + const Bias& biasHat) + : TangentPreintegration(p,biasHat) { + resetIntegration(); +} + +//------------------------------------------------------------------------------ +void TangentPreintegration::resetIntegration() { + deltaTij_ = 0.0; + preintegrated_.setZero(); + preintegrated_H_biasAcc_.setZero(); + preintegrated_H_biasOmega_.setZero(); +} + +//------------------------------------------------------------------------------ +bool TangentPreintegration::equals(const TangentPreintegration& other, + double tol) const { + return p_->equals(*other.p_, tol) + && fabs(deltaTij_ - other.deltaTij_) < tol + && biasHat_.equals(other.biasHat_, tol) + && equal_with_abs_tol(preintegrated_, other.preintegrated_, tol) + && equal_with_abs_tol(preintegrated_H_biasAcc_, other.preintegrated_H_biasAcc_, tol) + && equal_with_abs_tol(preintegrated_H_biasOmega_, other.preintegrated_H_biasOmega_, tol); +} + +//------------------------------------------------------------------------------ +// See extensive discussion in ImuFactor.lyx +Vector9 TangentPreintegration::UpdatePreintegrated( + const Vector3& a_body, const Vector3& w_body, double dt, + const Vector9& preintegrated, OptionalJacobian<9, 9> A, + OptionalJacobian<9, 3> B, OptionalJacobian<9, 3> C) { + const auto theta = preintegrated.segment<3>(0); + const auto position = preintegrated.segment<3>(3); + const auto velocity = preintegrated.segment<3>(6); + + // This functor allows for saving computation when exponential map and its + // derivatives are needed at the same location in so<3> + so3::DexpFunctor local(theta); + + // Calculate exact mean propagation + Matrix3 w_tangent_H_theta, invH; + const Vector3 w_tangent = // angular velocity mapped back to tangent space + local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0); + const SO3 R = local.expmap(); + const Vector3 a_nav = R * a_body; + const double dt22 = 0.5 * dt * dt; + + Vector9 preintegratedPlus; + preintegratedPlus << // new preintegrated vector: + theta + w_tangent* dt, // theta + position + velocity* dt + a_nav* dt22, // position + velocity + a_nav* dt; // velocity + + if (A) { + // Exact derivative of R*a with respect to theta: + const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp(); + + A->setIdentity(); + A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta + A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta... + A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity + A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta + } + if (B) { + B->block<3, 3>(0, 0) = Z_3x3; + B->block<3, 3>(3, 0) = R * dt22; + B->block<3, 3>(6, 0) = R * dt; + } + if (C) { + C->block<3, 3>(0, 0) = invH * dt; + C->block<3, 3>(3, 0) = Z_3x3; + C->block<3, 3>(6, 0) = Z_3x3; + } + + return preintegratedPlus; +} + +//------------------------------------------------------------------------------ +void TangentPreintegration::integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, + const double dt, Matrix9* A, + Matrix93* B, Matrix93* C) { + // Correct for bias in the sensor frame + Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); + Vector3 omega = biasHat_.correctGyroscope(measuredOmega); + + // Possibly correct for sensor pose + Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; + if (p().body_P_sensor) + boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, + D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); + + // Do update + deltaTij_ += dt; + preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C); + + if (p().body_P_sensor) { + // More complicated derivatives in case of non-trivial sensor pose + *C *= D_correctedOmega_omega; + if (!p().body_P_sensor->translation().isZero()) + *C += *B* D_correctedAcc_omega; + *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last + } + + // D_plus_abias = D_plus_preintegrated * D_preintegrated_abias + D_plus_a * D_a_abias + preintegrated_H_biasAcc_ = (*A) * preintegrated_H_biasAcc_ - (*B); + + // D_plus_wbias = D_plus_preintegrated * D_preintegrated_wbias + D_plus_w * D_w_wbias + preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C); +} + +void TangentPreintegration::integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, + double dt) { + // NOTE(frank): integrateMeasurement always needs to compute the derivatives, + // even when not of interest to the caller. Provide scratch space here. + Matrix9 A; + Matrix93 B, C; + integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); +} +//------------------------------------------------------------------------------ +Vector9 TangentPreintegration::biasCorrectedDelta( + const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { + // We correct for a change between bias_i and the biasHat_ used to integrate + // This is a simple linear correction with obvious derivatives + const imuBias::ConstantBias biasIncr = bias_i - biasHat_; + const Vector9 biasCorrected = + preintegrated() + preintegrated_H_biasAcc_ * biasIncr.accelerometer() + + preintegrated_H_biasOmega_ * biasIncr.gyroscope(); + + if (H) { + (*H) << preintegrated_H_biasAcc_, preintegrated_H_biasOmega_; + } + return biasCorrected; +} + +//------------------------------------------------------------------------------ +// sugar for derivative blocks +#define D_R_R(H) (H)->block<3,3>(0,0) +#define D_R_t(H) (H)->block<3,3>(0,3) +#define D_R_v(H) (H)->block<3,3>(0,6) +#define D_t_R(H) (H)->block<3,3>(3,0) +#define D_t_t(H) (H)->block<3,3>(3,3) +#define D_t_v(H) (H)->block<3,3>(3,6) +#define D_v_R(H) (H)->block<3,3>(6,0) +#define D_v_t(H) (H)->block<3,3>(6,3) +#define D_v_v(H) (H)->block<3,3>(6,6) + +//------------------------------------------------------------------------------ +Vector9 TangentPreintegration::Compose(const Vector9& zeta01, + const Vector9& zeta12, double deltaT12, + OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 9> H2) { + const auto t01 = zeta01.segment<3>(0); + const auto p01 = zeta01.segment<3>(3); + const auto v01 = zeta01.segment<3>(6); + + const auto t12 = zeta12.segment<3>(0); + const auto p12 = zeta12.segment<3>(3); + const auto v12 = zeta12.segment<3>(6); + + Matrix3 R01_H_t01, R12_H_t12; + const Rot3 R01 = Rot3::Expmap(t01, R01_H_t01); + const Rot3 R12 = Rot3::Expmap(t12, R12_H_t12); + + Matrix3 R02_H_R01, R02_H_R12; // NOTE(frank): R02_H_R12 == Identity + const Rot3 R02 = R01.compose(R12, R02_H_R01, R02_H_R12); + + Matrix3 t02_H_R02; + Vector9 zeta02; + const Matrix3 R = R01.matrix(); + zeta02 << Rot3::Logmap(R02, t02_H_R02), // theta + p01 + v01 * deltaT12 + R * p12, // position + v01 + R * v12; // velocity + + if (H1) { + H1->setIdentity(); + D_R_R(H1) = t02_H_R02 * R02_H_R01 * R01_H_t01; + D_t_R(H1) = R * skewSymmetric(-p12) * R01_H_t01; + D_t_v(H1) = I_3x3 * deltaT12; + D_v_R(H1) = R * skewSymmetric(-v12) * R01_H_t01; + } + + if (H2) { + H2->setZero(); + D_R_R(H2) = t02_H_R02 * R02_H_R12 * R12_H_t12; + D_t_t(H2) = R; + D_v_v(H2) = R; + } + + return zeta02; +} + +//------------------------------------------------------------------------------ +void TangentPreintegration::mergeWith(const TangentPreintegration& pim12, Matrix9* H1, + Matrix9* H2) { + if (!matchesParamsWith(pim12)) { + throw std::domain_error("Cannot merge pre-integrated measurements with different params"); + } + + if (params()->body_P_sensor) { + throw std::domain_error("Cannot merge pre-integrated measurements with sensor pose yet"); + } + + const double t01 = deltaTij(); + const double t12 = pim12.deltaTij(); + deltaTij_ = t01 + t12; + + const Vector9 zeta01 = preintegrated(); + Vector9 zeta12 = pim12.preintegrated(); // will be modified. + + const imuBias::ConstantBias bias_incr_for_12 = biasHat() - pim12.biasHat(); + zeta12 += pim12.preintegrated_H_biasOmega_ * bias_incr_for_12.gyroscope() + + pim12.preintegrated_H_biasAcc_ * bias_incr_for_12.accelerometer(); + + preintegrated_ = TangentPreintegration::Compose(zeta01, zeta12, t12, H1, H2); + + preintegrated_H_biasAcc_ = + (*H1) * preintegrated_H_biasAcc_ + (*H2) * pim12.preintegrated_H_biasAcc_; + + preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_ + + (*H2) * pim12.preintegrated_H_biasOmega_; +} + +//------------------------------------------------------------------------------ + +} // namespace gtsam diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h new file mode 100644 index 000000000..2d44be3ab --- /dev/null +++ b/gtsam/navigation/TangentPreintegration.h @@ -0,0 +1,143 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file TangentPreintegration.h + * @author Frank Dellaert + * @author Adam Bry + **/ + +#pragma once + +#include + +namespace gtsam { + +/** + * Integrate on the 9D tangent space of the NavState manifold. + * See extensive discussion in ImuFactor.lyx + */ +class TangentPreintegration : public PreintegrationBase { + protected: + + /** + * Preintegrated navigation state, as a 9D vector on tangent space at frame i + * Order is: theta, position, velocity + */ + Vector9 preintegrated_; + Matrix93 preintegrated_H_biasAcc_; ///< Jacobian of preintegrated preintegrated w.r.t. acceleration bias + Matrix93 preintegrated_H_biasOmega_; ///< Jacobian of preintegrated preintegrated w.r.t. angular rate bias + + /// Default constructor for serialization + TangentPreintegration() { + resetIntegration(); + } + +public: + /// @name Constructors + /// @{ + + /** + * Constructor, initializes the variables in the base class + * @param p Parameters, typically fixed in a single application + * @param bias Current estimate of acceleration and rotation rate biases + */ + TangentPreintegration(const boost::shared_ptr& p, + const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()); + + /// @} + + /// @name Basic utilities + /// @{ + /// Re-initialize PreintegratedMeasurements + void resetIntegration() override; + + /// @} + + /// @name Instance variables access + /// @{ + Vector3 deltaPij() const override { return preintegrated_.segment<3>(3); } + Vector3 deltaVij() const override { return preintegrated_.tail<3>(); } + Rot3 deltaRij() const override { return Rot3::Expmap(theta()); } + NavState deltaXij() const override { return NavState::Retract(preintegrated_); } + + const Vector9& preintegrated() const { return preintegrated_; } + Vector3 theta() const { return preintegrated_.head<3>(); } + const Matrix93& preintegrated_H_biasAcc() const { return preintegrated_H_biasAcc_; } + const Matrix93& preintegrated_H_biasOmega() const { return preintegrated_H_biasOmega_; } + + /// @name Testable + /// @{ + bool equals(const TangentPreintegration& other, double tol) const override; + /// @} + + /// @name Main functionality + /// @{ + + // Update integrated vector on tangent manifold preintegrated with acceleration + // Static, functional version. + static Vector9 UpdatePreintegrated(const Vector3& a_body, + const Vector3& w_body, const double dt, + const Vector9& preintegrated, + OptionalJacobian<9, 9> A = boost::none, + OptionalJacobian<9, 3> B = boost::none, + OptionalJacobian<9, 3> C = boost::none); + + // Version without derivatives + void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt); + + /// Update preintegrated measurements and get derivatives + /// It takes measured quantities in the j frame + /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose + /// NOTE(frank): implementation is different in two versions + void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, + Matrix9* A, Matrix93* B, Matrix93* C) override; + + /// Given the estimate of the bias, return a NavState tangent vector + /// summarizing the preintegrated IMU measurements so far + /// NOTE(frank): implementation is different in two versions + Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 6> H = boost::none) const override; + + // Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives + static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12, + double deltaT12, + OptionalJacobian<9, 9> H1 = boost::none, + OptionalJacobian<9, 9> H2 = boost::none); + + /// Merge in a different set of measurements and update bias derivatives accordingly + /// The derivatives apply to the preintegrated Vector9 + void mergeWith(const TangentPreintegration& pim, Matrix9* H1, Matrix9* H2); + /// @} + + /** Dummy clone for MATLAB */ + virtual boost::shared_ptr clone() const { + return boost::shared_ptr(); + } + + /// @} + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; + ar & BOOST_SERIALIZATION_NVP(p_); + ar & BOOST_SERIALIZATION_NVP(biasHat_); + ar & BOOST_SERIALIZATION_NVP(deltaTij_); + ar & bs::make_nvp("preintegrated_", bs::make_array(preintegrated_.data(), preintegrated_.size())); + ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size())); + ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size())); + } +}; + +} /// namespace gtsam diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 29624e70d..c1c17a6d4 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -132,7 +132,7 @@ TEST( CombinedImuFactor, ErrorWithBiases ) { } /* ************************************************************************* */ -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION +#ifdef GTSAM_TANGENT_PREINTEGRATION TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) { auto p = testing::Params(); testing::SomeMeasurements measurements; diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index c060b4c9c..8748a7fcf 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -445,7 +445,7 @@ TEST(ImuFactor, fistOrderExponential) { } /* ************************************************************************* */ -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION +#ifdef GTSAM_TANGENT_PREINTEGRATION TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { testing::SomeMeasurements measurements; @@ -796,7 +796,7 @@ TEST(ImuFactor, bodyPSensorWithBias) { } /* ************************************************************************* */ -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION +#ifdef GTSAM_TANGENT_PREINTEGRATION static const double kVelocity = 2.0, kAngularVelocity = M_PI / 6; struct ImuFactorMergeTest { diff --git a/gtsam/navigation/tests/testPreintegrationBase.cpp b/gtsam/navigation/tests/testTangentPreintegration.cpp similarity index 82% rename from gtsam/navigation/tests/testPreintegrationBase.cpp rename to gtsam/navigation/tests/testTangentPreintegration.cpp index 4a32faef8..af6e283bd 100644 --- a/gtsam/navigation/tests/testPreintegrationBase.cpp +++ b/gtsam/navigation/tests/testTangentPreintegration.cpp @@ -10,12 +10,12 @@ * -------------------------------------------------------------------------- */ /** - * @file testPreintegrationBase.cpp + * @file testTangentPreintegration.cpp * @brief Unit test for the InertialNavFactor * @author Frank Dellaert */ -#include +#include #include #include #include @@ -26,11 +26,10 @@ #include "imuFactorTesting.h" -#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION static const double kDt = 0.1; Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) { - return PreintegrationBase::UpdatePreintegrated(a, w, kDt, zeta); + return TangentPreintegration::UpdatePreintegrated(a, w, kDt, zeta); } namespace testing { @@ -45,8 +44,8 @@ static boost::shared_ptr Params() { } /* ************************************************************************* */ -TEST(PreintegrationBase, UpdateEstimate1) { - PreintegrationBase pim(testing::Params()); +TEST(TangentPreintegration, UpdateEstimate1) { + TangentPreintegration pim(testing::Params()); const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); Vector9 zeta; zeta.setZero(); @@ -59,8 +58,8 @@ TEST(PreintegrationBase, UpdateEstimate1) { } /* ************************************************************************* */ -TEST(PreintegrationBase, UpdateEstimate2) { - PreintegrationBase pim(testing::Params()); +TEST(TangentPreintegration, UpdateEstimate2) { + TangentPreintegration pim(testing::Params()); const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); Vector9 zeta; zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3; @@ -74,8 +73,8 @@ TEST(PreintegrationBase, UpdateEstimate2) { } /* ************************************************************************* */ -TEST(PreintegrationBase, computeError) { - PreintegrationBase pim(testing::Params()); +TEST(TangentPreintegration, computeError) { + TangentPreintegration pim(testing::Params()); NavState x1, x2; imuBias::ConstantBias bias; Matrix9 aH1, aH2; @@ -83,7 +82,7 @@ TEST(PreintegrationBase, computeError) { pim.computeError(x1, x2, bias, aH1, aH2, aH3); boost::function f = - boost::bind(&PreintegrationBase::computeError, pim, _1, _2, _3, + boost::bind(&TangentPreintegration::computeError, pim, _1, _2, _3, boost::none, boost::none, boost::none); // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); @@ -92,47 +91,47 @@ TEST(PreintegrationBase, computeError) { } /* ************************************************************************* */ -TEST(PreintegrationBase, Compose) { +TEST(TangentPreintegration, Compose) { testing::SomeMeasurements measurements; - PreintegrationBase pim(testing::Params()); + TangentPreintegration pim(testing::Params()); testing::integrateMeasurements(measurements, &pim); boost::function f = [pim](const Vector9& zeta01, const Vector9& zeta12) { - return PreintegrationBase::Compose(zeta01, zeta12, pim.deltaTij()); + return TangentPreintegration::Compose(zeta01, zeta12, pim.deltaTij()); }; // Expected merge result - PreintegrationBase expected_pim02(testing::Params()); + TangentPreintegration expected_pim02(testing::Params()); testing::integrateMeasurements(measurements, &expected_pim02); testing::integrateMeasurements(measurements, &expected_pim02); // Actual result Matrix9 H1, H2; - PreintegrationBase actual_pim02 = pim; + TangentPreintegration actual_pim02 = pim; actual_pim02.mergeWith(pim, &H1, &H2); const Vector9 zeta = pim.preintegrated(); const Vector9 actual_zeta = - PreintegrationBase::Compose(zeta, zeta, pim.deltaTij()); + TangentPreintegration::Compose(zeta, zeta, pim.deltaTij()); EXPECT(assert_equal(expected_pim02.preintegrated(), actual_zeta, 1e-7)); EXPECT(assert_equal(numericalDerivative21(f, zeta, zeta), H1, 1e-7)); EXPECT(assert_equal(numericalDerivative22(f, zeta, zeta), H2, 1e-7)); } /* ************************************************************************* */ -TEST(PreintegrationBase, MergedBiasDerivatives) { +TEST(TangentPreintegration, MergedBiasDerivatives) { testing::SomeMeasurements measurements; auto f = [=](const Vector3& a, const Vector3& w) { - PreintegrationBase pim02(testing::Params(), Bias(a, w)); + TangentPreintegration pim02(testing::Params(), Bias(a, w)); testing::integrateMeasurements(measurements, &pim02); testing::integrateMeasurements(measurements, &pim02); return pim02.preintegrated(); }; // Expected merge result - PreintegrationBase expected_pim02(testing::Params()); + TangentPreintegration expected_pim02(testing::Params()); testing::integrateMeasurements(measurements, &expected_pim02); testing::integrateMeasurements(measurements, &expected_pim02); @@ -141,7 +140,6 @@ TEST(PreintegrationBase, MergedBiasDerivatives) { EXPECT(assert_equal(numericalDerivative22(f, Z_3x1, Z_3x1), expected_pim02.preintegrated_H_biasOmega(), 1e-7)); } -#endif /* ************************************************************************* */ int main() {