diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index b65810aae..42ea41b86 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/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 //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 5fbd619cf..f7b6aa43f 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/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 diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 2a4e00048..c25086eea 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/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 @@ -51,8 +52,8 @@ namespace { PreintegratedCombinedMeasurements evaluatePreintegratedMeasurements( const imuBias::ConstantBias& bias, const list& measuredAccs, const list& measuredOmegas, const list& deltaTs) { - PreintegratedCombinedMeasurements result(bias, I_3x3, - I_3x3, I_3x3, I_3x3, I_3x3, I_6x6); + auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81); + PreintegratedCombinedMeasurements result(p, bias); list::const_iterator itAcc = measuredAccs.begin(); list::const_iterator itOmega = measuredOmegas.begin();