12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Made api backwards compatible.

release/4.3a0
krunalchande 2015-03-08 22:07:14 -04:00
parent 736fce27db
commit ab21ee9507
4 changed files with 19 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/navigation/CombinedImuFactor.h
View File

@ -213,12 +213,14 @@ public:
boost::optional<Matrix&> H6 = boost::none) const; boost::optional<Matrix&> H6 = boost::none) const;
/** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */ /** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */
static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i, 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 imuBias::ConstantBias& bias_i, const CombinedPreintegratedMeasurements& PIM, const Vector3& gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none, boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) { boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) {
return 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;
} }

8
gtsam/navigation/ImuFactor.h
View File

@ -195,12 +195,14 @@ public:
boost::optional<Matrix&> H5 = boost::none) const; boost::optional<Matrix&> H5 = boost::none) const;
/** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */ /** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */
static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i, 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 imuBias::ConstantBias& bias_i, const PreintegratedMeasurements& PIM, const Vector3& gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none, boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) { boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) {
return 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;
} }
private: private:

8
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -320,11 +320,11 @@ TEST(CombinedImuFactor, PredictRotation) {
Combined_pre_int_data, gravity, omegaCoriolis); Combined_pre_int_data, gravity, omegaCoriolis);
// Predict // Predict
Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0)); Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0)), x2;
Vector3 v(0,0,0); Vector3 v(0,0,0), v2;
PoseVelocityBias poseVelocityBias = CombinedImuFactor::Predict(x,v,bias, Combinedfactor.preintegratedMeasurements(), gravity, omegaCoriolis); CombinedImuFactor::Predict(x, v, x2, v2, bias, Combinedfactor.preintegratedMeasurements(), gravity, omegaCoriolis);
Pose3 expectedPose(Rot3().ypr(M_PI/10, 0,0), Point3(0,0,0)); 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));
} }
/* ************************************************************************* */ /* ************************************************************************* */

11
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -913,13 +913,14 @@ TEST(ImuFactor, PredictRotation) {
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
// Predict // Predict
Pose3 x1; Pose3 x1, x2;
Vector3 v1(0, 0.0, 0.0); Vector3 v1 = Vector3(0, 0.0, 0.0);
PoseVelocityBias poseVelocity = ImuFactor::Predict(x1, v1, bias, factor.preintegratedMeasurements(), gravity, omegaCoriolis); 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)); Pose3 expectedPose(Rot3().ypr(M_PI/10, 0, 0), Point3(0, 0, 0));
Vector3 expectedVelocity; expectedVelocity<<0,0,0; Vector3 expectedVelocity; expectedVelocity<<0,0,0;
EXPECT(assert_equal(expectedPose, poseVelocity.pose)); EXPECT(assert_equal(expectedPose, x2));
EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); EXPECT(assert_equal(Vector(expectedVelocity), Vector(v2)));
} }
/* ************************************************************************* */ /* ************************************************************************* */