moved prediction to base class

gtsam/navigation/CombinedImuFactor.cpp

@@ -385,50 +385,4 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_
   return r;
 }
 
-//------------------------------------------------------------------------------
-PoseVelocityBias CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
-    const imuBias::ConstantBias& bias_i,
-    const PreintegrationBase& preintegratedMeasurements,
-    const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){
-
-  const double& deltaTij = preintegratedMeasurements.deltaTij_;
-  const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer();
-  const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope();
-
-  const Rot3 Rot_i = pose_i.rotation();
-  const Vector3 pos_i = pose_i.translation().vector();
-
-  // Predict state at time j
-  /* ---------------------------------------------------------------------------------------------------- */
-  Vector3 pos_j =  pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_
-      + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr
-      + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr)
-      + vel_i * deltaTij
-      - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
-      + 0.5 * gravity * deltaTij*deltaTij;
-
-  Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
-      + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
-      + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
-      - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij  // Coriolis term
-      + gravity * deltaTij);
-
-  if(use2ndOrderCoriolis){
-    pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij;  // 2nd order coriolis term for position
-    vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
-  }
-
-  const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
-  // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
-  Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
-  Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
-      Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
-  const Rot3 deltaRij_biascorrected_corioliscorrected =
-      Rot3::Expmap( theta_biascorrected_corioliscorrected );
-  const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected  );
-
-  Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
-  return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant
-}
-
 } /// namespace gtsam

gtsam/navigation/CombinedImuFactor.h

@@ -45,20 +45,6 @@ namespace gtsam {
  * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013.
  */
 
-/**
- * Struct to hold all state variables of CombinedImuFactor returned by Predict function
- */
-struct PoseVelocityBias {
-  Pose3 pose;
-  Vector3 velocity;
-  imuBias::ConstantBias bias;
-
-  PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
-      const imuBias::ConstantBias _bias) :
-        pose(_pose), velocity(_velocity), bias(_bias) {
-  }
-};
-
 /**
  * CombinedImuFactor is a 6-ways factor involving previous state (pose and velocity of the vehicle, as well as bias
  * at previous time step), and current state (pose, velocity, bias at current time step). According to the
@@ -133,7 +119,7 @@ public:
 
     /// methods to access class variables
     Matrix measurementCovariance() const {return measurementCovariance_;}
-    Matrix PreintMeasCov() const { return preintMeasCov_;}
+    Matrix preintMeasCov() const { return preintMeasCov_;}
 
   private:
 
@@ -219,7 +205,9 @@ public:
   static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i,
       const imuBias::ConstantBias& bias_i,
       const PreintegrationBase& preintegratedMeasurements,
-      const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
+      const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false){
+    return ImuFactorBase::Predict(pose_i, vel_i, bias_i, preintegratedMeasurements, gravity, omegaCoriolis, use2ndOrderCoriolis);
+  }
 
 private:
 

gtsam/navigation/ImuFactor.cpp

@@ -327,50 +327,4 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const
   return r;
 }
 
-//------------------------------------------------------------------------------
-PoseVelocityBias ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
-    const imuBias::ConstantBias& bias_i,
-    const PreintegrationBase& preintegratedMeasurements,
-    const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){
-
-  const double& deltaTij = preintegratedMeasurements.deltaTij_;
-  const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer();
-  const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope();
-
-  const Rot3 Rot_i = pose_i.rotation();
-  const Vector3 pos_i = pose_i.translation().vector();
-
-  // Predict state at time j
-  /* ---------------------------------------------------------------------------------------------------- */
-  Vector3 pos_j =  pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_
-      + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr
-      + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr)
-      + vel_i * deltaTij
-      - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
-      + 0.5 * gravity * deltaTij*deltaTij;
-
-  Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
-      + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
-      + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
-      - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij  // Coriolis term
-      + gravity * deltaTij);
-
-  if(use2ndOrderCoriolis){
-    pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij;  // 2nd order coriolis term for position
-    vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
-  }
-
-  const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
-  // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
-  Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
-  Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
-      Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
-  const Rot3 deltaRij_biascorrected_corioliscorrected =
-      Rot3::Expmap( theta_biascorrected_corioliscorrected );
-  const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected  );
-
-  Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
-  return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant
-}
-
 } /// namespace gtsam

gtsam/navigation/ImuFactor.h

@@ -45,20 +45,6 @@ namespace gtsam {
  * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013.
  */
 
-/**
- * Struct to hold return variables by the Predict Function
- */
-struct PoseVelocityBias {
-  Pose3 pose;
-  Vector3 velocity;
-  imuBias::ConstantBias bias;
-
-  PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
-      const imuBias::ConstantBias _bias) :
-        pose(_pose), velocity(_velocity), bias(_bias) {
-  }
-};
-
 /**
  * ImuFactor is a 5-ways factor involving previous state (pose and velocity of the vehicle at previous time step),
  * current state (pose and velocity at current time step), and the bias estimate. According to the
@@ -207,8 +193,10 @@ public:
 
   /// predicted states from IMU
   static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i,
-      const imuBias::ConstantBias& bias, const PreintegrationBase& preintegratedMeasurements,
+      const imuBias::ConstantBias& bias_i, const PreintegrationBase& preintegratedMeasurements,
-      const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
+      const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false){
+    return ImuFactorBase::Predict(pose_i, vel_i, bias_i, preintegratedMeasurements, gravity, omegaCoriolis, use2ndOrderCoriolis);
+  }
 
   private:
 

gtsam/navigation/ImuFactorBase.h

@@ -26,6 +26,20 @@
 
 namespace gtsam {
 
+/**
+ * Struct to hold all state variables of returned by Predict function
+ */
+struct PoseVelocityBias {
+  Pose3 pose;
+  Vector3 velocity;
+  imuBias::ConstantBias bias;
+
+  PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
+      const imuBias::ConstantBias _bias) :
+        pose(_pose), velocity(_velocity), bias(_bias) {
+  }
+};
+
 class ImuFactorBase {
 
 protected:
@@ -69,6 +83,53 @@ public:
         (body_P_sensor_ && expected.body_P_sensor_ && body_P_sensor_->equals(*expected.body_P_sensor_)));
   }
 
+  /// Predict state at time j
+  //------------------------------------------------------------------------------
+  static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i,
+      const imuBias::ConstantBias& bias_i,
+      const PreintegrationBase& preintegratedMeasurements,
+      const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){
+
+    const double& deltaTij = preintegratedMeasurements.deltaTij_;
+    const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer();
+    const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope();
+
+    const Rot3 Rot_i = pose_i.rotation();
+    const Vector3 pos_i = pose_i.translation().vector();
+
+    // Predict state at time j
+    /* ---------------------------------------------------------------------------------------------------- */
+    Vector3 pos_j =  pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_
+        + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr
+        + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr)
+        + vel_i * deltaTij
+        - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
+        + 0.5 * gravity * deltaTij*deltaTij;
+
+    Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
+        + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
+        + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
+        - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij  // Coriolis term
+        + gravity * deltaTij);
+
+    if(use2ndOrderCoriolis){
+      pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij;  // 2nd order coriolis term for position
+      vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
+    }
+
+    const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
+    // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
+    Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
+    Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
+        Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
+    const Rot3 deltaRij_biascorrected_corioliscorrected =
+        Rot3::Expmap( theta_biascorrected_corioliscorrected );
+    const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected  );
+
+    Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
+    return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant
+  }
+
 };
 
 } /// namespace gtsam

gtsam/navigation/PreintegrationBase.h

@@ -34,6 +34,7 @@ namespace gtsam {
  */
 class PreintegrationBase {
 
+  friend class ImuFactorBase;
   friend class ImuFactor;
   friend class CombinedImuFactor;
 

gtsam/navigation/tests/testImuFactor.cpp

@@ -561,13 +561,11 @@ TEST(ImuFactor, PredictPositionAndVelocity){
     // Predict
     Pose3 x1;
     Vector3 v1(0, 0.0, 0.0);
-    PoseVelocity poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis);
+    PoseVelocityBias poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis);
     Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
     Vector3 expectedVelocity; expectedVelocity<<0,1,0;
     EXPECT(assert_equal(expectedPose, poseVelocity.pose));
     EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity)));
-
-
 }
 
 /* ************************************************************************* */
@@ -595,7 +593,7 @@ TEST(ImuFactor, PredictRotation) {
     // Predict
     Pose3 x1;
     Vector3 v1(0, 0.0, 0.0);
-    PoseVelocity poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis);
+    PoseVelocityBias poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis);
     Pose3 expectedPose(Rot3().ypr(M_PI/10, 0, 0), Point3(0, 0, 0));
     Vector3 expectedVelocity; expectedVelocity<<0,0,0;
     EXPECT(assert_equal(expectedPose, poseVelocity.pose));