Drastic simplification triumph !

gtsam/navigation/PreintegrationBase.cpp

@@ -38,11 +38,8 @@ PreintegrationBase::PreintegrationBase(const boost::shared_ptr<Params>& p,
 void PreintegrationBase::resetIntegration() {
   deltaTij_ = 0.0;
   deltaXij_ = TangentVector();
-  delRdelBiasOmega_ = Z_3x3;
+  zeta_H_biasAcc_.setZero();
-  delPdelBiasAcc_ = Z_3x3;
+  zeta_H_biasOmega_.setZero();
-  delPdelBiasOmega_ = Z_3x3;
-  delVdelBiasAcc_ = Z_3x3;
-  delVdelBiasOmega_ = Z_3x3;
 }
 
 //------------------------------------------------------------------------------
@@ -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(zeta_H_biasAcc_, other.zeta_H_biasAcc_, tol)
-      && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol)
+      && equal_with_abs_tol(zeta_H_biasOmega_, other.zeta_H_biasOmega_, 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);
 }
 
 //------------------------------------------------------------------------------
@@ -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;
+  zeta_H_biasAcc_ = (*A) * zeta_H_biasAcc_ - (*B);
-  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);
 
   // D_plus_wbias = D_plus_zeta * D_zeta_wbias + D_plus_w * D_w_wbias
-  Matrix93 D_zeta_wbias, D_plus_wbias;
+  zeta_H_biasOmega_ = (*A) * zeta_H_biasOmega_ - (*C);
-  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);
 }
 
 //------------------------------------------------------------------------------
 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 Vector9 xi = zeta() + zeta_H_biasAcc_ * biasIncr.accelerometer() +
-  const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope();
+                     zeta_H_biasOmega_ * 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;
+    (*H) << zeta_H_biasAcc_, zeta_H_biasOmega_;
-    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;
 }

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
+  Matrix93 zeta_H_biasAcc_;    ///< Jacobian of preintegrated zeta w.r.t. acceleration bias
-  Matrix3 delPdelBiasAcc_;   ///< Jacobian of preintegrated position w.r.t. acceleration bias
+  Matrix93 zeta_H_biasOmega_;  ///< Jacobian of preintegrated zeta w.r.t. angular rate 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
   PreintegrationBase() {
@@ -144,21 +141,6 @@ public:
   PreintegrationBase(const boost::shared_ptr<Params>& p,
       const imuBias::ConstantBias& biasHat = imuBias::ConstantBias());
 
-  /**
-   *  Constructor which takes in all members for generic construction
-   */
-  PreintegrationBase(const boost::shared_ptr<Params>& 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() {
+  const Matrix93& zeta_H_biasAcc() const { return zeta_H_biasAcc_; }
-    return (Matrix93() << Z_3x3, delPdelBiasAcc_, delVdelBiasAcc_).finished();
+  const Matrix93& zeta_H_biasOmega() const { return zeta_H_biasOmega_; }
-  }
-  const Matrix93 zeta_H_biasOmega() {
-    return (Matrix93() << delRdelBiasOmega_, delPdelBiasOmega_, delVdelBiasOmega_).finished();
-  }
 
   // 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("zeta_H_biasAcc_", bs::make_array(zeta_H_biasAcc_.data(), zeta_H_biasAcc_.size()));
-    ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size()));
+    ar & bs::make_nvp("zeta_H_biasOmega_", bs::make_array(zeta_H_biasOmega_.data(), zeta_H_biasOmega_.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()));
   }
 };