Deprecated constructor + fixed parameter name

gtsam/navigation/CombinedImuFactor.cpp

@@ -108,16 +108,16 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
   // BLOCK DIAGONAL TERMS
   D_t_t(&G_measCov_Gt) = dt * iCov;
   D_v_v(&G_measCov_Gt) = (1 / dt) * H_vel_biasacc *
-                         (aCov + p().biasAccOmegaInit.block<3, 3>(0, 0)) *
+                         (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0)) *
                          (H_vel_biasacc.transpose());
   D_R_R(&G_measCov_Gt) = (1 / dt) * H_angles_biasomega *
-                         (wCov + p().biasAccOmegaInit.block<3, 3>(3, 3)) *
+                         (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3)) *
                          (H_angles_biasomega.transpose());
   D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
   D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance;
 
   // OFF BLOCK DIAGONAL TERMS
-  Matrix3 temp = H_vel_biasacc * p().biasAccOmegaInit.block<3, 3>(3, 0) *
+  Matrix3 temp = H_vel_biasacc * p().biasAccOmegaInt.block<3, 3>(3, 0) *
                  H_angles_biasomega.transpose();
   D_v_R(&G_measCov_Gt) = temp;
   D_R_v(&G_measCov_Gt) = temp.transpose();
@@ -125,11 +125,12 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
 }
 
 //------------------------------------------------------------------------------
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
 PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements(
     const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance,
     const Matrix3& measuredOmegaCovariance,
     const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance,
-    const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInit,
+    const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInt,
     const bool use2ndOrderIntegration) {
   if (!use2ndOrderIntegration)
     throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
@@ -140,11 +141,12 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements(
   p->integrationCovariance = integrationErrorCovariance;
   p->biasAccCovariance = biasAccCovariance;
   p->biasOmegaCovariance = biasOmegaCovariance;
-  p->biasAccOmegaInit = biasAccOmegaInit;
+  p->biasAccOmegaInt = biasAccOmegaInt;
   p_ = p;
   resetIntegration();
   preintMeasCov_.setZero();
 }
+#endif
 //------------------------------------------------------------------------------
 // CombinedImuFactor methods
 //------------------------------------------------------------------------------

gtsam/navigation/CombinedImuFactor.h

@@ -66,12 +66,12 @@ public:
   struct Params : PreintegrationParams {
     Matrix3 biasAccCovariance;    ///< continuous-time "Covariance" describing accelerometer bias random walk
     Matrix3 biasOmegaCovariance;  ///< continuous-time "Covariance" describing gyroscope bias random walk
-    Matrix6 biasAccOmegaInit;     ///< covariance of bias used for pre-integration
+    Matrix6 biasAccOmegaInt;     ///< covariance of bias used for pre-integration
 
     /// See two named constructors below for good values of n_gravity in body frame
     Params(const Vector3& n_gravity) :
         PreintegrationParams(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance(
-            I_3x3), biasAccOmegaInit(I_6x6) {
+            I_3x3), biasAccOmegaInt(I_6x6) {
     }
 
     // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
@@ -95,7 +95,7 @@ public:
       ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params);
       ar& BOOST_SERIALIZATION_NVP(biasAccCovariance);
       ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
-      ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInit);
+      ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
     }
   };
 
@@ -166,9 +166,7 @@ public:
 
   /// @}
 
-  /// @name Deprecated
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
-  /// @{
-
   /// deprecated constructor
   /// NOTE(frank): assumes Z-Down convention, only second order integration supported
   PreintegratedCombinedMeasurements(const imuBias::ConstantBias& biasHat,
@@ -176,9 +174,8 @@ public:
       const Matrix3& measuredOmegaCovariance,
       const Matrix3& integrationErrorCovariance,
       const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
-      const Matrix6& biasAccOmegaInit, const bool use2ndOrderIntegration = true);
+      const Matrix6& biasAccOmegaInt, const bool use2ndOrderIntegration = true);
-
+#endif
-  /// @}
 
  private:
   /// Serialization function

gtsam/navigation/tests/testCombinedImuFactor.cpp

@@ -13,8 +13,9 @@
  * @file    testCombinedImuFactor.cpp
  * @brief   Unit test for Lupton-style combined IMU factor
  * @author  Luca Carlone
- * @author  Stephen Williams
+ * @author  Frank Dellaert
  * @author  Richard Roberts
+ * @author  Stephen Williams
  */
 
 #include <gtsam/navigation/ImuFactor.h>
@@ -51,8 +52,8 @@ namespace {
 PreintegratedCombinedMeasurements evaluatePreintegratedMeasurements(
     const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
     const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
-  PreintegratedCombinedMeasurements result(bias, I_3x3,
+  auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81);
-      I_3x3, I_3x3, I_3x3, I_3x3, I_6x6);
+  PreintegratedCombinedMeasurements result(p, bias);
 
   list<Vector3>::const_iterator itAcc = measuredAccs.begin();
   list<Vector3>::const_iterator itOmega = measuredOmegas.begin();