Removed inheritance from PreintegratedRotation

gtsam/navigation/ImuFactor.cpp

@@ -81,10 +81,10 @@ void PreintegratedImuMeasurements::integrateMeasurement(
   // 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();
-  D_t_t(&preintMeasCov_) += p().integrationCovariance * deltaT;
-  D_v_v(&preintMeasCov_) += dRij * p().accelerometerCovariance * dRij.transpose() * deltaT;
   D_R_R(&preintMeasCov_) += D_incrR_integratedOmega * p().gyroscopeCovariance
       * D_incrR_integratedOmega.transpose() * deltaT;
+  D_t_t(&preintMeasCov_) += p().integrationCovariance * deltaT;
+  D_v_v(&preintMeasCov_) += dRij * p().accelerometerCovariance * dRij.transpose() * deltaT;
 
   Matrix3 F_pos_noiseacc;
   F_pos_noiseacc = 0.5 * dRij * deltaT * deltaT;

gtsam/navigation/PreintegrationBase.cpp

@@ -28,7 +28,9 @@ namespace gtsam {
 
 /// Re-initialize PreintegratedMeasurements
 void PreintegrationBase::resetIntegration() {
-  PreintegratedRotation::resetIntegration();
+  deltaTij_ = 0.0;
+  deltaRij_ = Rot3();
+  delRdelBiasOmega_ = Z_3x3;
   deltaPij_ = Vector3::Zero();
   deltaVij_ = Vector3::Zero();
   delPdelBiasAcc_ = Z_3x3;
@@ -39,7 +41,9 @@ void PreintegrationBase::resetIntegration() {
 
 /// Needed for testable
 void PreintegrationBase::print(const string& s) const {
-  PreintegratedRotation::print(s);
+  cout << s << endl;
+  cout << "    deltaTij [" << deltaTij_ << "]" << endl;
+  cout << "    deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << endl;
   cout << "    deltaPij [ " << deltaPij_.transpose() << " ]" << endl;
   cout << "    deltaVij [ " << deltaVij_.transpose() << " ]" << endl;
   biasHat_.print("    biasHat");
@@ -48,7 +52,9 @@ void PreintegrationBase::print(const string& s) const {
 /// Needed for testable
 bool PreintegrationBase::equals(const PreintegrationBase& other,
     double tol) const {
-  return PreintegratedRotation::equals(other, tol)
+  return deltaRij_.equals(other.deltaRij_, tol)
+      && fabs(deltaTij_ - other.deltaTij_) < tol
+      && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol)
       && biasHat_.equals(other.biasHat_, tol)
       && equal_with_abs_tol(deltaPij_, other.deltaPij_, tol)
       && equal_with_abs_tol(deltaVij_, other.deltaVij_, tol)
@@ -130,7 +136,9 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
   // Correct deltaRij, derivative is delRdelBiasOmega_
   const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
   Matrix3 D_deltaRij_bias;
-  Rot3 deltaRij = biascorrectedDeltaRij(biasIncr.gyroscope(), H ? &D_deltaRij_bias : 0);
+  const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope();
+  const Rot3 deltaRij = deltaRij_.expmap(biasInducedOmega, boost::none, H ? &D_deltaRij_bias : 0);
+  if (H) D_deltaRij_bias *= delRdelBiasOmega_;
 
   Vector9 xi;
   Matrix3 D_dR_deltaRij;

gtsam/navigation/PreintegrationBase.h

@@ -51,7 +51,7 @@ struct PoseVelocityBias {
  * It includes the definitions of the preintegrated variables and the methods
  * to access, print, and compare them.
  */
-class PreintegrationBase: public PreintegratedRotation {
+class PreintegrationBase {
 
 public:
 
@@ -100,6 +100,13 @@ public:
 
 protected:
 
+  double deltaTij_;  ///< Time interval from i to j
+  Rot3 deltaRij_;    ///< Preintegrated relative orientation (in frame i)
+  Matrix3 delRdelBiasOmega_;  ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
+
+  /// Parameters
+  boost::shared_ptr<Params> p_;
+
   /// Acceleration and gyro bias used for preintegration
   imuBias::ConstantBias biasHat_;
 
@@ -124,7 +131,7 @@ public:
    */
   PreintegrationBase(const boost::shared_ptr<Params>& p,
       const imuBias::ConstantBias& biasHat) :
-      PreintegratedRotation(p), biasHat_(biasHat) {
+      p_(p), biasHat_(biasHat) {
     resetIntegration();
   }
 
@@ -136,6 +143,15 @@ public:
   }
 
   /// getters
+  const double& deltaTij() const {
+    return deltaTij_;
+  }
+  const Rot3& deltaRij() const {
+    return deltaRij_;
+  }
+  const Matrix3& delRdelBiasOmega() const {
+    return delRdelBiasOmega_;
+  }
   const imuBias::ConstantBias& biasHat() const {
     return biasHat_;
   }
@@ -202,8 +218,10 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation);
     ar & BOOST_SERIALIZATION_NVP(p_);
+    ar & BOOST_SERIALIZATION_NVP(deltaTij_);
+    ar & BOOST_SERIALIZATION_NVP(deltaRij_);
+    ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
     ar & BOOST_SERIALIZATION_NVP(biasHat_);
     ar & BOOST_SERIALIZATION_NVP(deltaPij_);
     ar & BOOST_SERIALIZATION_NVP(deltaVij_);

gtsam/navigation/tests/testImuFactor.cpp

@@ -364,15 +364,7 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
       kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
   pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
-  // Make sure of biascorrectedDeltaRij with this example...
+  // Make sure of biasCorrectedDelta
-  Matrix3 aH;
-  pim.biascorrectedDeltaRij(bias.gyroscope(), aH);
-  Matrix3 eH = numericalDerivative11<Rot3, Vector3>(
-      boost::bind(&PreintegrationBase::biascorrectedDeltaRij, pim, _1,
-          boost::none), bias.gyroscope());
-  EXPECT(assert_equal(eH, aH));
-
-  // ... and of biasCorrectedDelta
   Matrix96 actualH;
   pim.biasCorrectedDelta(bias, actualH);
   Matrix expectedH = numericalDerivative11<Vector9, imuBias::ConstantBias>(
@@ -803,11 +795,14 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
 Matrix9 MonteCarlo(const PreintegratedImuMeasurements& pim,
     const NavState& state, const imuBias::ConstantBias& bias, double dt,
     const Pose3& body_P_sensor, const Vector3& measuredAcc,
-    const Vector3& measuredOmega, size_t N = 1000) {
+    const Vector3& measuredOmega, size_t N = 100) {
   // Get mean prediction from "ground truth" measurements
   PreintegratedImuMeasurements pim1 = pim;
-  pim1.integrateMeasurement(measuredAcc, measuredOmega, dt, body_P_sensor);
+  for (size_t k=0;k<10;k++)
+    pim1.integrateMeasurement(measuredAcc, measuredOmega, dt, body_P_sensor);
   NavState mean = pim1.predict(state, bias);
+  cout << "pim1.preintMeasCov():" << endl;
+  cout << pim1.preintMeasCov() << endl;
 
   // Do a Monte Carlo analysis to determine empirical density on the predicted state
   Sampler sampleAccelerationNoise(Vector3::Constant(sqrt(accNoiseVar)));
@@ -818,18 +813,23 @@ Matrix9 MonteCarlo(const PreintegratedImuMeasurements& pim,
     PreintegratedImuMeasurements pim2 = pim;
     Vector3 perturbedAcc = measuredAcc + sampleAccelerationNoise.sample();
     Vector3 perturbedOmega = measuredOmega + sampleOmegaNoise.sample();
-    pim2.integrateMeasurement(perturbedAcc, perturbedOmega, dt, body_P_sensor);
+    for (size_t k=0;k<10;k++)
+      pim2.integrateMeasurement(perturbedAcc, perturbedOmega, dt, body_P_sensor);
     NavState prediction = pim2.predict(state, bias);
     samples.col(i) = mean.localCoordinates(prediction);
     sum += samples.col(i);
   }
   Vector9 sampleMean = sum / N;
+  cout << ":" << endl;
+  cout << sampleMean << endl;
   Matrix9 Q;
   Q.setZero();
   for (size_t i = 0; i < N; i++) {
     Vector9 xi = samples.col(i) - sampleMean;
     Q += xi * xi.transpose() / (N - 1);
   }
+  cout << "Q:" << endl;
+  cout << Q << endl;
   return Q;
 }
 
@@ -857,8 +857,6 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
       kMeasuredOmegaCovariance, kIntegrationErrorCovariance, true);
   Matrix9 Q = MonteCarlo(pim, NavState(x1, v1), biasHat, dt, body_P_sensor,
       measuredAcc, measuredOmega, 1000);
-  cout << Q << endl;
-
 
   Matrix expected(9,9);
   expected <<
@@ -993,14 +991,10 @@ TEST(ImuFactor, PredictArbitrary) {
       kIntegrationErrorCovariance, true);
   Pose3 x1;
   Vector3 v1 = Vector3(0, 0, 0);
-  Matrix9 Q = MonteCarlo(pim, NavState(x1, v1), biasHat, dt, Pose3(),
+  Matrix9 Q = MonteCarlo(pim, NavState(x1, v1), biasHat, 0.1, Pose3(),
       measuredAcc, measuredOmega, 1000);
-  cout << Q << endl;
 
-  pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
+  for (int i = 0; i < 1000; ++i)
-  cout << pim.preintMeasCov() << endl;
-
-  for (int i = 0; i < 999; ++i)
     pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
 
   Matrix expected(9,9);