diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index a7c6ecd39..d2ad32122 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -212,6 +212,18 @@ public: boost::optional H5 = boost::none, boost::optional H6 = boost::none) const; + /** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */ + 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, const Vector3& gravity, + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, + boost::optional deltaPij_biascorrected_out = boost::none, + boost::optional deltaVij_biascorrected_out = boost::none) { + PoseVelocityBias PVB(PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out)); + pose_j = PVB.pose; + vel_j = PVB.velocity; + } + + private: /** Serialization function */ diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index b91c76ade..984d62d08 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -194,6 +194,17 @@ public: boost::optional H4 = boost::none, boost::optional H5 = boost::none) const; + /** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */ + 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, + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, + boost::optional deltaPij_biascorrected_out = boost::none, + boost::optional deltaVij_biascorrected_out = boost::none) { + PoseVelocityBias PVB(PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out)); + pose_j = PVB.pose; + vel_j = PVB.velocity; + } + private: /** Serialization function */ diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 3f7109543..c6eb1396e 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -320,11 +320,11 @@ TEST(CombinedImuFactor, PredictRotation) { Combined_pre_int_data, gravity, omegaCoriolis); // Predict - Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0)); - Vector3 v(0,0,0); - PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x,v,bias, gravity, omegaCoriolis); + Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0)), x2; + Vector3 v(0,0,0), v2; + CombinedImuFactor::Predict(x, v, x2, v2, bias, Combinedfactor.preintegratedMeasurements(), gravity, omegaCoriolis); Pose3 expectedPose(Rot3().ypr(M_PI/10, 0,0), Point3(0,0,0)); - EXPECT(assert_equal(expectedPose, poseVelocityBias.pose, tol)); + EXPECT(assert_equal(expectedPose, x2, tol)); } /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 9c82a7dfa..62f496894 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -913,13 +913,14 @@ TEST(ImuFactor, PredictRotation) { ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); // Predict - Pose3 x1; - Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); + Pose3 x1, x2; + Vector3 v1 = Vector3(0, 0.0, 0.0); + Vector3 v2; + ImuFactor::Predict(x1, v1, x2, v2, bias, factor.preintegratedMeasurements(), 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)); - EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); + EXPECT(assert_equal(expectedPose, x2)); + EXPECT(assert_equal(Vector(expectedVelocity), Vector(v2))); } /* ************************************************************************* */