diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index c1ec7f361..620305d77 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -277,19 +277,13 @@ CombinedImuFactor::CombinedImuFactor( void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i, - const CombinedPreintegratedMeasurements& pim, + CombinedPreintegratedMeasurements& pim, const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) { - // NOTE(frank): hack below only for backward compatibility - boost::shared_ptr p = - boost::make_shared(pim.p()); - p->gravity = gravity; - p->omegaCoriolis = omegaCoriolis; - p->use2ndOrderCoriolis = use2ndOrderCoriolis; - CombinedPreintegratedMeasurements newPim = pim; - newPim.p_ = p; - PoseVelocityBias pvb = newPim.predict(pose_i, vel_i, bias_i); + // use deprecated predict + PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, gravity, + omegaCoriolis, use2ndOrderCoriolis); pose_j = pvb.pose; vel_j = pvb.velocity; } diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index a70d1d24f..5641b4c3e 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -245,7 +245,7 @@ public: // @deprecated use PreintegrationBase::predict static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i, - const CombinedPreintegratedMeasurements& pim, + CombinedPreintegratedMeasurements& pim, const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 024bdd65e..8f7e28385 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -195,18 +195,12 @@ ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i, - const PreintegratedMeasurements& pim, + PreintegratedMeasurements& pim, const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) { - // NOTE(frank): hack below only for backward compatibility - boost::shared_ptr p = - boost::make_shared(pim.p()); - p->gravity = gravity; - p->omegaCoriolis = omegaCoriolis; - p->use2ndOrderCoriolis = use2ndOrderCoriolis; - PreintegratedMeasurements newPim = pim; - newPim.p_ = p; - PoseVelocityBias pvb = newPim.predict(pose_i, vel_i, bias_i); + // use deprecated predict + PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, gravity, + omegaCoriolis, use2ndOrderCoriolis); pose_j = pvb.pose; vel_j = pvb.velocity; } diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index eb94675fa..9cd1fcada 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -212,7 +212,7 @@ public: /// @deprecated use PreintegrationBase::predict static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i, - const PreintegratedMeasurements& pim, const Vector3& gravity, + PreintegratedMeasurements& pim, const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); private: