From 2a244cac12a039dc1416acad4beae83c4e7e43e2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 30 Jan 2016 11:58:11 -0800 Subject: [PATCH] Drastic simplification triumph ! --- gtsam/navigation/PreintegrationBase.cpp | 54 +++++-------------------- gtsam/navigation/PreintegrationBase.h | 37 +++-------------- 2 files changed, 17 insertions(+), 74 deletions(-) diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 2a64074ff..57afbcdbe 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -38,11 +38,8 @@ PreintegrationBase::PreintegrationBase(const boost::shared_ptr& p, void PreintegrationBase::resetIntegration() { deltaTij_ = 0.0; deltaXij_ = TangentVector(); - delRdelBiasOmega_ = Z_3x3; - delPdelBiasAcc_ = Z_3x3; - delPdelBiasOmega_ = Z_3x3; - delVdelBiasAcc_ = Z_3x3; - delVdelBiasOmega_ = Z_3x3; + zeta_H_biasAcc_.setZero(); + zeta_H_biasOmega_.setZero(); } //------------------------------------------------------------------------------ @@ -68,11 +65,8 @@ bool PreintegrationBase::equals(const PreintegrationBase& other, 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(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); + && equal_with_abs_tol(zeta_H_biasAcc_, other.zeta_H_biasAcc_, tol) + && equal_with_abs_tol(zeta_H_biasOmega_, other.zeta_H_biasOmega_, tol); } //------------------------------------------------------------------------------ @@ -226,49 +220,23 @@ void PreintegrationBase::update(const Vector3& measuredAcc, deltaXij_ = updatedDeltaXij(measuredAcc, measuredOmega, dt, A, B, C); // D_plus_abias = D_plus_zeta * D_zeta_abias + D_plus_a * D_a_abias - Matrix93 D_zeta_abias, D_plus_abias; - D_zeta_abias << Z_3x3, delPdelBiasAcc_, delVdelBiasAcc_; - D_plus_abias = (*A) * D_zeta_abias - (*B); - delPdelBiasAcc_ = D_plus_abias.middleRows<3>(3); - delVdelBiasAcc_ = D_plus_abias.middleRows<3>(6); + zeta_H_biasAcc_ = (*A) * zeta_H_biasAcc_ - (*B); // D_plus_wbias = D_plus_zeta * D_zeta_wbias + D_plus_w * D_w_wbias - Matrix93 D_zeta_wbias, D_plus_wbias; - D_zeta_wbias << delRdelBiasOmega_, delPdelBiasOmega_, delVdelBiasOmega_; - D_plus_wbias = (*A) * D_zeta_wbias - (*C); - delRdelBiasOmega_ = D_plus_wbias.middleRows<3>(0); - delPdelBiasOmega_ = D_plus_wbias.middleRows<3>(3); - delVdelBiasOmega_ = D_plus_wbias.middleRows<3>(6); + zeta_H_biasOmega_ = (*A) * zeta_H_biasOmega_ - (*C); } //------------------------------------------------------------------------------ Vector9 PreintegrationBase::biasCorrectedDelta( const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { -// Correct deltaRij, derivative is delRdelBiasOmega_ + // 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_; - 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(); + const Vector9 xi = zeta() + zeta_H_biasAcc_ * biasIncr.accelerometer() + + zeta_H_biasOmega_ * 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; + (*H) << zeta_H_biasAcc_, zeta_H_biasOmega_; } return xi; } diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index f2d031dc0..3a4d99752 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -121,11 +121,8 @@ class PreintegrationBase { /// Current estimate of deltaXij_k TangentVector 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 + 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 /// Default constructor for serialization PreintegrationBase() { @@ -144,21 +141,6 @@ public: PreintegrationBase(const boost::shared_ptr& p, const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()); - /** - * Constructor which takes in all members for generic construction - */ - PreintegrationBase(const boost::shared_ptr& p, const imuBias::ConstantBias& biasHat, - double deltaTij, const TangentVector& zeta, const Matrix3& delPdelBiasAcc, - const Matrix3& delPdelBiasOmega, const Matrix3& delVdelBiasAcc, - const Matrix3& delVdelBiasOmega) - : p_(p), - biasHat_(biasHat), - deltaTij_(deltaTij), - deltaXij_(zeta), - delPdelBiasAcc_(delPdelBiasAcc), - delPdelBiasOmega_(delPdelBiasOmega), - delVdelBiasAcc_(delVdelBiasAcc), - delVdelBiasOmega_(delVdelBiasOmega) {} /// @} /// @name Basic utilities @@ -202,12 +184,8 @@ public: Rot3 deltaRij() const { return Rot3::Expmap(deltaXij_.theta()); } NavState deltaXij() const { return NavState::Retract(deltaXij_.vector()); } - const Matrix93 zeta_H_biasAcc() { - return (Matrix93() << Z_3x3, delPdelBiasAcc_, delVdelBiasAcc_).finished(); - } - const Matrix93 zeta_H_biasOmega() { - return (Matrix93() << delRdelBiasOmega_, delPdelBiasOmega_, delVdelBiasOmega_).finished(); - } + const Matrix93& zeta_H_biasAcc() const { return zeta_H_biasAcc_; } + const Matrix93& zeta_H_biasOmega() const { return zeta_H_biasOmega_; } // Exposed for MATLAB Vector6 biasHatVector() const { return biasHat_.vector(); } @@ -309,11 +287,8 @@ private: 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())); + 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())); } };