diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 6eb4de58b..d2ad32122 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -218,7 +218,7 @@ public: 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)); + 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; } diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 4369a411f..984d62d08 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -200,7 +200,7 @@ public: 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)); + 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; } diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 3c30082f6..29b9b6e66 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -199,7 +199,7 @@ public: /// Predict state at time j //------------------------------------------------------------------------------ - PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i, + PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, boost::optional deltaPij_biascorrected_out = boost::none, @@ -273,7 +273,7 @@ public: // Evaluate residual error, according to [3] /* ---------------------------------------------------------------------------------------------------- */ Vector3 deltaPij_biascorrected, deltaVij_biascorrected; - PoseVelocityBias predictedState_j = Predict(pose_i, vel_i, bias_i, gravity, + PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected, deltaVij_biascorrected); // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 70686acfd..c6eb1396e 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -294,7 +294,7 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity){ // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocityBias = Combined_pre_int_data.Predict(x1, v1, bias, gravity, omegaCoriolis); + PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); Vector3 expectedVelocity; expectedVelocity<<0,1,0; EXPECT(assert_equal(expectedPose, poseVelocityBias.pose)); diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index b1dbd504f..62f496894 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -883,7 +883,7 @@ TEST(ImuFactor, PredictPositionAndVelocity){ // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocity = pre_int_data.Predict(x1, v1, bias, gravity, omegaCoriolis); + PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); Vector3 expectedVelocity; expectedVelocity<<0,1,0; EXPECT(assert_equal(expectedPose, poseVelocity.pose));