diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 57afbcdbe..c3c73e9d8 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -37,7 +37,7 @@ PreintegrationBase::PreintegrationBase(const boost::shared_ptr& p, //------------------------------------------------------------------------------ void PreintegrationBase::resetIntegration() { deltaTij_ = 0.0; - deltaXij_ = TangentVector(); + zeta_ = Vector9(); zeta_H_biasAcc_.setZero(); zeta_H_biasOmega_.setZero(); } @@ -64,7 +64,7 @@ bool PreintegrationBase::equals(const PreintegrationBase& other, const bool params_match = p_->equals(*other.p_, tol); return params_match && fabs(deltaTij_ - other.deltaTij_) < tol && biasHat_.equals(other.biasHat_, tol) - && equal_with_abs_tol(deltaXij_.vector(), other.deltaXij_.vector(), tol) + && equal_with_abs_tol(zeta_, other.zeta_, tol) && equal_with_abs_tol(zeta_H_biasAcc_, other.zeta_H_biasAcc_, tol) && equal_with_abs_tol(zeta_H_biasOmega_, other.zeta_H_biasOmega_, tol); } @@ -120,42 +120,52 @@ pair PreintegrationBase::correctMeasurementsByBiasAndSensorPos //------------------------------------------------------------------------------ // See extensive discussion in ImuFactor.lyx -PreintegrationBase::TangentVector PreintegrationBase::UpdateEstimate( - const Vector3& a_body, const Vector3& w_body, double dt, - const TangentVector& zeta, OptionalJacobian<9, 9> A, - OptionalJacobian<9, 3> B, OptionalJacobian<9, 3> C) { +Vector9 PreintegrationBase::UpdateEstimate(const Vector3& a_body, + const Vector3& w_body, double dt, + const Vector9& zeta, + OptionalJacobian<9, 9> A, + OptionalJacobian<9, 3> B, + OptionalJacobian<9, 3> C) { + const auto theta = zeta.segment<3>(0); + const auto position = zeta.segment<3>(3); + const auto velocity = zeta.segment<3>(6); + // Calculate exact mean propagation Matrix3 H; - const Matrix3 R = Rot3::Expmap(zeta.theta(), H).matrix(); + const Matrix3 R = Rot3::Expmap(theta, H).matrix(); const Matrix3 invH = H.inverse(); const Vector3 a_nav = R * a_body; const double dt22 = 0.5 * dt * dt; - TangentVector zetaPlus(zeta.theta() + invH * w_body * dt, - zeta.position() + zeta.velocity() * dt + a_nav * dt22, - zeta.velocity() + a_nav * dt); + Vector9 zetaPlus; + zetaPlus << // + theta + invH* w_body* dt, // theta + position + velocity* dt + a_nav* dt22, // position + velocity + a_nav* dt; // velocity if (A) { +#define USE_NUMERICAL_DERIVATIVE +#ifdef USE_NUMERICAL_DERIVATIVE + auto f = [w_body](const Vector3& theta) { + return Rot3::ExpmapDerivative(theta).inverse() * w_body; + }; + const Matrix3 invHw_H_theta = + numericalDerivative11(f, theta); +#else // First order (small angle) approximation of derivative of invH*w: -// const Matrix3 invHw_H_theta = skewSymmetric(-0.5 * w_body); - // NOTE(frank): Rot3::ExpmapDerivative(w_body) is also an approximation (but less accurate): - // If we replace approximation with numerical derivative we get better accuracy: - auto f = [w_body](const Vector3& theta) { - return Rot3::ExpmapDerivative(theta).inverse() * w_body; - }; - const Matrix3 invHw_H_theta = numericalDerivative11(f, zeta.theta()); + // TODO(frank): find a cheap closed form solution (look at Iserles) + // NOTE(frank): Rot3::ExpmapDerivative(w_body) is a less accurate approximation + const Matrix3 invHw_H_theta = skewSymmetric(-0.5 * w_body); +#endif // Exact derivative of R*a with respect to theta: const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * H; A->setIdentity(); - // theta: - A->block<3, 3>(0, 0).noalias() += invHw_H_theta * dt; - // position: - A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; - A->block<3, 3>(3, 6) = I_3x3 * dt; - // velocity: - A->block<3, 3>(6, 0) = a_nav_H_theta * dt; + A->block<3, 3>(0, 0).noalias() += invHw_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; @@ -172,7 +182,7 @@ PreintegrationBase::TangentVector PreintegrationBase::UpdateEstimate( } //------------------------------------------------------------------------------ -PreintegrationBase::TangentVector PreintegrationBase::updatedDeltaXij( +Vector9 PreintegrationBase::updatedZeta( const Vector3& measuredAcc, const Vector3& measuredOmega, double dt, OptionalJacobian<9, 9> A, OptionalJacobian<9, 3> B, OptionalJacobian<9, 3> C) const { @@ -182,7 +192,7 @@ PreintegrationBase::TangentVector PreintegrationBase::updatedDeltaXij( const Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); // Do update in one fell swoop - return UpdateEstimate(correctedAcc, correctedOmega, dt, deltaXij_, A, B, C); + return UpdateEstimate(correctedAcc, correctedOmega, dt, zeta_, A, B, C); } else { // More complicated derivatives in case of sensor displacement Vector3 correctedAcc, correctedOmega; @@ -196,8 +206,8 @@ PreintegrationBase::TangentVector PreintegrationBase::updatedDeltaXij( // Do update in one fell swoop Matrix93 D_updated_correctedAcc, D_updated_correctedOmega; - const PreintegrationBase::TangentVector updated = - UpdateEstimate(correctedAcc, correctedOmega, dt, deltaXij_, A, + const Vector9 updated = + UpdateEstimate(correctedAcc, correctedOmega, dt, zeta_, A, ((B || C) ? &D_updated_correctedAcc : 0), (C ? &D_updated_correctedOmega : 0)); if (B) *B = D_updated_correctedAcc* D_correctedAcc_measuredAcc; @@ -217,7 +227,7 @@ void PreintegrationBase::update(const Vector3& measuredAcc, Matrix93* B, Matrix93* C) { // Do update deltaTij_ += dt; - deltaXij_ = updatedDeltaXij(measuredAcc, measuredOmega, dt, A, B, C); + zeta_ = updatedZeta(measuredAcc, measuredOmega, dt, A, B, C); // D_plus_abias = D_plus_zeta * D_zeta_abias + D_plus_a * D_a_abias zeta_H_biasAcc_ = (*A) * zeta_H_biasAcc_ - (*B); diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 3a4d99752..110f1ae1d 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -60,44 +60,6 @@ class PreintegrationBase { typedef imuBias::ConstantBias Bias; typedef PreintegrationParams Params; - /// The IMU is integrated in the tangent space, represented by a Vector9 - /// This small inner class provides some convenient constructors and efficient - /// access to the orientation, position, and velocity components - class TangentVector { - Vector9 v_; - typedef const Vector9 constV9; - - public: - TangentVector() { v_.setZero(); } - TangentVector(const Vector9& v) : v_(v) {} - TangentVector(const Vector3& theta, const Vector3& pos, - const Vector3& vel) { - this->theta() = theta; - this->position() = pos; - this->velocity() = vel; - } - - const Vector9& vector() const { return v_; } - - Eigen::Block theta() { return v_.segment<3>(0); } - Eigen::Block theta() const { return v_.segment<3>(0); } - - Eigen::Block position() { return v_.segment<3>(3); } - Eigen::Block position() const { return v_.segment<3>(3); } - - Eigen::Block velocity() { return v_.segment<3>(6); } - Eigen::Block velocity() const { return v_.segment<3>(6); } - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - namespace bs = ::boost::serialization; - ar & bs::make_nvp("v_", bs::make_array(v_.data(), v_.size())); - } - }; - protected: /// Parameters. Declared mutable only for deprecated predict method. @@ -108,7 +70,7 @@ class PreintegrationBase { boost::shared_ptr p_; /// Acceleration and gyro bias used for preintegration - imuBias::ConstantBias biasHat_; + Bias biasHat_; /// Time interval from i to j double deltaTij_; @@ -118,8 +80,7 @@ class PreintegrationBase { * 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] */ - /// Current estimate of deltaXij_k - TangentVector deltaXij_; + Vector9 zeta_; Matrix93 zeta_H_biasAcc_; ///< Jacobian of preintegrated zeta w.r.t. acceleration bias Matrix93 zeta_H_biasOmega_; ///< Jacobian of preintegrated zeta w.r.t. angular rate bias @@ -175,14 +136,14 @@ public: const imuBias::ConstantBias& biasHat() const { return biasHat_; } const double& deltaTij() const { return deltaTij_; } - const Vector9& zeta() const { return deltaXij_.vector(); } + const Vector9& zeta() const { return zeta_; } - Vector3 theta() const { return deltaXij_.theta(); } - Vector3 deltaPij() const { return deltaXij_.position(); } - Vector3 deltaVij() const { return deltaXij_.velocity(); } + Vector3 theta() const { return zeta_.head<3>(); } + Vector3 deltaPij() const { return zeta_.segment<3>(3); } + Vector3 deltaVij() const { return zeta_.tail<3>(); } - Rot3 deltaRij() const { return Rot3::Expmap(deltaXij_.theta()); } - NavState deltaXij() const { return NavState::Retract(deltaXij_.vector()); } + Rot3 deltaRij() const { return Rot3::Expmap(theta()); } + NavState deltaXij() const { return NavState::Retract(zeta_); } const Matrix93& zeta_H_biasAcc() const { return zeta_H_biasAcc_; } const Matrix93& zeta_H_biasOmega() const { return zeta_H_biasOmega_; } @@ -212,20 +173,18 @@ public: // Update integrated vector on tangent manifold zeta with acceleration // readings a_body and gyro readings w_body, bias-corrected in body frame. - static TangentVector UpdateEstimate(const Vector3& a_body, - const Vector3& w_body, double dt, - const TangentVector& zeta, - OptionalJacobian<9, 9> A = boost::none, - OptionalJacobian<9, 3> B = boost::none, - OptionalJacobian<9, 3> C = boost::none); + static Vector9 UpdateEstimate(const Vector3& a_body, const Vector3& w_body, + double dt, const Vector9& zeta, + OptionalJacobian<9, 9> A = boost::none, + OptionalJacobian<9, 3> B = boost::none, + OptionalJacobian<9, 3> C = boost::none); /// Calculate the updated preintegrated measurement, does not modify /// It takes measured quantities in the j frame - PreintegrationBase::TangentVector updatedDeltaXij( - const Vector3& measuredAcc, const Vector3& measuredOmega, double dt, - OptionalJacobian<9, 9> A = boost::none, - OptionalJacobian<9, 3> B = boost::none, - OptionalJacobian<9, 3> C = boost::none) const; + Vector9 updatedZeta(const Vector3& measuredAcc, const Vector3& measuredOmega, + double dt, OptionalJacobian<9, 9> A = boost::none, + OptionalJacobian<9, 3> B = boost::none, + OptionalJacobian<9, 3> C = boost::none) const; /// Update preintegrated measurements and get derivatives /// It takes measured quantities in the j frame @@ -284,9 +243,9 @@ private: 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 & BOOST_SERIALIZATION_NVP(deltaTij_); + ar & bs::make_nvp("zeta_", bs::make_array(zeta_.data(), zeta_.size())); ar & bs::make_nvp("zeta_H_biasAcc_", bs::make_array(zeta_H_biasAcc_.data(), zeta_H_biasAcc_.size())); ar & bs::make_nvp("zeta_H_biasOmega_", bs::make_array(zeta_H_biasOmega_.data(), zeta_H_biasOmega_.size())); } diff --git a/gtsam/navigation/tests/testPreintegrationBase.cpp b/gtsam/navigation/tests/testPreintegrationBase.cpp index bc67091ae..3327bfdb6 100644 --- a/gtsam/navigation/tests/testPreintegrationBase.cpp +++ b/gtsam/navigation/tests/testPreintegrationBase.cpp @@ -39,9 +39,7 @@ static boost::shared_ptr defaultParams() { } Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) { - PreintegrationBase::TangentVector zeta_plus = - PreintegrationBase::UpdateEstimate(a, w, kDt, zeta); - return zeta_plus.vector(); + return PreintegrationBase::UpdateEstimate(a, w, kDt, zeta); } /* ************************************************************************* */