truth revealing unit test :-) re-established good functioning of IMU factor (TODO: fix CombinedImuFactor F & G)

gtsam/navigation/ImuFactor.cpp

@@ -67,7 +67,8 @@ void ImuFactor::PreintegratedMeasurements::resetIntegration(){
 //------------------------------------------------------------------------------
 void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
     const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
-    boost::optional<const Pose3&> body_P_sensor) {
+    boost::optional<const Pose3&> body_P_sensor,
+    boost::optional<Matrix&> Fout, boost::optional<Matrix&> Gout) {
 
   // NOTE: order is important here because each update uses old values (i.e., we have to update
   // jacobians and covariances before updating preintegrated measurements).
@@ -87,6 +88,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
   // as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework
   /* ----------------------------------------------------------------------------------------------------------------------- */
   const Vector3 theta_i = thetaRij(); // super-expensive parametrization of so(3)
+  const Matrix3 R_i = deltaRij();
   const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i);
 
   // Update preintegrated measurements. TODO Frank moved from end of this function !!!
@@ -102,7 +104,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
 
   Matrix H_vel_pos    = Z_3x3;
   Matrix H_vel_vel    = I_3x3;
-  Matrix H_vel_angles = - deltaRij() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
+  Matrix H_vel_angles = - R_i * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
   // analytic expression corresponding to the following numerical derivative
   // Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
 
@@ -124,15 +126,20 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
   // Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT
   preintMeasCov_ = F * preintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ;
 
+  // Fout and Gout are used for testing purposes and are not needed by the factor
+  if(Fout){
+    Fout->resize(9,9);
+    (*Fout) << F;
+  }
+  if(Gout){
     // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT
-  // This in only kept for documentation.
+    // This in only kept for testing.
-  //
+    Gout->resize(9,9);
-  // Matrix G(9,9);
+    (*Gout) << I_3x3 * deltaT, Z_3x3,        Z_3x3,
-  // G << I_3x3 * deltaT, Z_3x3,  Z_3x3,
+               Z_3x3,          R_i * deltaT, Z_3x3,
-  //      Z_3x3, deltaRij.matrix() * deltaT, Z_3x3,
+               Z_3x3,          Z_3x3,        Jrinv_theta_j * Jr_theta_incr * deltaT;
-  //      Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;
+    //preintMeasCov = F * preintMeasCov * F.transpose() + Gout * (1/deltaT) * measurementCovariance * Gout.transpose();
-  //
+  }
-  // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose();
 }
 
 //------------------------------------------------------------------------------

gtsam/navigation/ImuFactor.h

@@ -111,9 +111,11 @@ public:
      * @param measuredOmega Measured angular velocity (as given by the sensor)
      * @param deltaT Time interval between two consecutive IMU measurements
      * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
+     * @param Fout, Gout Jacobians used internally (only needed for testing)
      */
     void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
-        boost::optional<const Pose3&> body_P_sensor = boost::none);
+        boost::optional<const Pose3&> body_P_sensor = boost::none,
+        boost::optional<Matrix&> Fout = boost::none, boost::optional<Matrix&> Gout = boost::none);
 
     /// methods to access class variables
     Matrix measurementCovariance() const {return measurementCovariance_;}

gtsam/navigation/PreintegrationBase.h

@@ -343,28 +343,6 @@ public:
     return r;
   }
 
-  /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
-  // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
-  static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
-      const Vector3& delta_angles, const Vector& delta_vel_in_t0){
-    // Note: all delta terms refer to an IMU\sensor system at t0
-    Vector body_t_a_body = msr_acc_t;
-    Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
-    return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
-  }
-
-  // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
-  static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
-      const Vector3& delta_angles){
-    // Note: all delta terms refer to an IMU\sensor system at t0
-    // Calculate the corrected measurements using the Bias object
-    Vector body_t_omega_body= msr_gyro_t;
-    Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
-    R_t_to_t0    = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
-    return Rot3::Logmap(R_t_to_t0);
-  }
-
-  /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
 private:
   /** Serialization function */
   friend class boost::serialization::access;

gtsam/navigation/tests/testImuFactor.cpp

@@ -49,6 +49,23 @@ Rot3 evaluateRotationError(const ImuFactor& factor,
   return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ;
 }
 
+Vector PreIntegrateIMUObservations_delta_vel(const Vector3& msr_acc_t,
+    const double msr_dt, const Vector3& delta_angles, const Vector3& delta_vel_in_t0){
+  // Note: all delta terms refer to an IMU\sensor system at t0
+  Vector body_t_a_body = msr_acc_t;
+  Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
+  return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
+}
+
+Vector PreIntegrateIMUObservations_delta_angles(const Vector3& msr_gyro_t,
+    const double msr_dt, const Vector3& delta_angles){
+  // Note: all delta terms refer to an IMU\sensor system at t0
+  // Calculate the corrected measurements using the Bias object
+  Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles) * Rot3::Expmap( msr_gyro_t * msr_dt );
+  Vector result = Rot3::Logmap(R_t_to_t0);
+  return result;
+}
+
 ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
     const imuBias::ConstantBias& bias,
     const list<Vector3>& measuredAccs,
@@ -423,6 +440,75 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
   EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
 }
 
+/* ************************************************************************* */
+TEST( ImuFactor, JacobianPreintegratedCovariancePropagation )
+{
+  // Linearization point
+  imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
+  Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1));
+
+  // Measurements
+  list<Vector3> measuredAccs, measuredOmegas;
+  list<double> deltaTs;
+  measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
+  measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
+  deltaTs.push_back(0.01);
+  measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
+  measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
+  deltaTs.push_back(0.01);
+  for(int i=1;i<100;i++)
+  {
+    measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
+    measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0));
+    deltaTs.push_back(0.01);
+  }
+  // Actual preintegrated values
+  ImuFactor::PreintegratedMeasurements preintegrated =
+      evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0));
+
+  // so far we only created a nontrivial linearization point for the preintegrated measurements
+  // Now we add a new measurement and ask for Jacobians
+  const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0);
+  const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0);
+  const double newDeltaT = 0.01;
+  const Vector3 theta_i = preintegrated.thetaRij(); // before adding new measurement
+  const Vector3 deltaVij = preintegrated.deltaVij();// before adding new measurement
+
+  Matrix Factual, Gactual;
+  preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT,
+      body_P_sensor, Factual, Gactual);
+
+  // Compute expected F
+  Matrix H_vel_angles_expected =
+      numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel,
+      newMeasuredAcc, newDeltaT, _1, deltaVij), theta_i);
+  Matrix H_angles_angles_expected =
+  numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles,
+      newMeasuredOmega, newDeltaT, _1), theta_i);
+  Matrix Fexpected(9,9);
+  Fexpected << I_3x3,    I_3x3 * newDeltaT,  Z_3x3,
+               Z_3x3,    I_3x3, H_vel_angles_expected,
+               Z_3x3, Z_3x3, H_angles_angles_expected;
+
+  // verify F
+  EXPECT(assert_equal(Fexpected, Factual));
+
+  // Compute expected G
+  Matrix H_vel_noiseAcc_expected =
+      numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel,
+          _1, newDeltaT, theta_i, deltaVij), newMeasuredAcc);
+  Matrix H_angles_noiseOmega_expected =
+      numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles,
+          _1, newDeltaT, theta_i), newMeasuredOmega);
+  Matrix Gexpected(9,9);
+  //  [integrationError measuredAcc measuredOmega]
+  Gexpected << I_3x3 * newDeltaT,  Z_3x3,                   Z_3x3,
+               Z_3x3,              H_vel_noiseAcc_expected, Z_3x3,
+               Z_3x3,              Z_3x3,                   H_angles_noiseOmega_expected;
+  // verify G
+  EXPECT(assert_equal(Gexpected, Gactual,1e-7));
+}
+
 //#include <gtsam/linear/GaussianFactorGraph.h>
 ///* ************************************************************************* */
 //TEST( ImuFactor, LinearizeTiming)