From 1ab1323a33b28e434c1053687ac9cb4683d34d10 Mon Sep 17 00:00:00 2001 From: krunalchande Date: Fri, 21 Nov 2014 20:02:39 -0500 Subject: [PATCH] Added unit tests for Predict --- gtsam/navigation/AHRSFactor.h | 7 +-- gtsam/navigation/ImuFactor.h | 17 ++++-- gtsam/navigation/tests/testAHRSFactor.cpp | 10 ++-- gtsam/navigation/tests/testImuFactor.cpp | 65 +++++++++++++++++++++++ 4 files changed, 88 insertions(+), 11 deletions(-) diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 19277a26a..444ddc2b0 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -83,7 +83,7 @@ public: tol); } Matrix measurementCovariance() const {return measurementCovariance_;} - Matrix deltaRij() const {return deltaRij_.matrix();} + Rot3 deltaRij() const {return deltaRij_;} double deltaTij() const {return deltaTij_;} Vector biasHat() const {return biasHat_.vector();} Matrix3 delRdelBiasOmega() {return delRdelBiasOmega_;} @@ -349,7 +349,7 @@ public: } /** predicted states from IMU */ - static void predict(const Rot3& rot_i, const Rot3& rot_j, + static Rot3 predict(const Rot3& rot_i, const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, const Vector3& omegaCoriolis, @@ -376,7 +376,8 @@ public: - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term const Rot3 deltaRij_biascorrected_corioliscorrected = Rot3::Expmap( theta_biascorrected_corioliscorrected); - const Rot3 Rot_j = Rot_i.compose(deltaRij_biascorrected_corioliscorrected); +// const Rot3 Rot_j = + return (Rot_i.compose(deltaRij_biascorrected_corioliscorrected)); } diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index d69dd29e7..f22e483ce 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -28,6 +28,16 @@ namespace gtsam { +/** + * Struct to hold return variables by the Predict Function + */ +struct PoseVelocity { + Pose3 pose; + Vector3 velocity; + PoseVelocity(const Pose3& _pose, const Vector3& _velocity) : + pose(_pose), velocity(_velocity) { + } +}; /** * @@ -547,7 +557,7 @@ namespace gtsam { /** predicted states from IMU */ - static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, + static PoseVelocity Predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) @@ -569,7 +579,7 @@ namespace gtsam { - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + 0.5 * gravity * deltaTij*deltaTij; - vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ + Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr) - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term @@ -589,7 +599,8 @@ namespace gtsam { Rot3::Expmap( theta_biascorrected_corioliscorrected ); const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); - pose_j = Pose3( Rot_j, Point3(pos_j) ); + Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); + return PoseVelocity(pose_j, vel_j); } diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index d573dbe42..e82586141 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -69,7 +69,7 @@ Rot3 evaluatePreintegratedMeasurementsRotation( const list& deltaTs, const Vector3& initialRotationRate = Vector3(0.0, 0.0, 0.0)) { return evaluatePreintegratedMeasurements(bias, measuredOmegas, - deltaTs, initialRotationRate).deltaRij_; + deltaTs, initialRotationRate).deltaRij(); } Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, @@ -99,8 +99,8 @@ TEST( AHRSFactor, PreintegratedMeasurements ) { AHRSFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero()); actual1.integrateMeasurement(measuredOmega, deltaT); - EXPECT(assert_equal(expectedDeltaR1, actual1.deltaRij_, 1e-6)); - DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij_, 1e-6); + EXPECT(assert_equal(expectedDeltaR1, actual1.deltaRij(), 1e-6)); + DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6); // Integrate again Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0); @@ -110,8 +110,8 @@ TEST( AHRSFactor, PreintegratedMeasurements ) { AHRSFactor::PreintegratedMeasurements actual2 = actual1; actual2.integrateMeasurement(measuredOmega, deltaT); - EXPECT(assert_equal(expectedDeltaR2, actual2.deltaRij_, 1e-6)); - DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij_, 1e-6); + EXPECT(assert_equal(expectedDeltaR2, actual2.deltaRij(), 1e-6)); + DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6); } /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 0a1f1e104..bcfa79663 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -560,6 +560,71 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) EXPECT(assert_equal(H5e, H5a)); } +TEST(ImuFactor, PredictPositionAndVelocity){ + imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) + + // Measurements + Vector3 gravity; gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; + Vector3 measuredOmega; measuredOmega << 0, 0, 0;//M_PI/10.0+0.3; + Vector3 measuredAcc; measuredAcc << 0,1,-9.81; + double deltaT = 0.001; + double tol = 1e-6; + + Matrix I6x6(6,6); + I6x6 = Matrix::Identity(6,6); + + ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), + Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), true); + + for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // Create factor + 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); + PoseVelocity poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis); + Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); + Vector3 expectedVelocity; expectedVelocity<<0,1,0; + EXPECT(assert_equal(expectedPose, poseVelocity.pose)); + EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); + + +} + +TEST(ImuFactor, PredictRotation) { + imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) + + // Measurements + Vector3 gravity; gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; + Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10;//M_PI/10.0+0.3; + Vector3 measuredAcc; measuredAcc << 0,0,-9.81; + double deltaT = 0.001; + double tol = 1e-6; + + Matrix I6x6(6,6); + I6x6 = Matrix::Identity(6,6); + + ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), + Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), true); + + for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // Create factor + 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); + PoseVelocity poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, 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))); +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);}