From 00b83ced7aabd41cfe895fdc046792d356f20a8a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Dec 2015 11:15:42 -0800 Subject: [PATCH] AcceleratingScenario + some refactoring (v and a specified in nav frame) --- gtsam/navigation/Scenario.h | 71 ++++++++++++------- gtsam/navigation/ScenarioRunner.h | 6 +- gtsam/navigation/tests/testScenario.cpp | 53 ++++++++++++-- gtsam/navigation/tests/testScenarioRunner.cpp | 24 +++++++ 4 files changed, 118 insertions(+), 36 deletions(-) diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index 58d6057b3..69b5dfa00 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -54,25 +54,25 @@ class Scenario { Matrix3 gyroCovariance() const { return gyroNoiseModel_->covariance(); } Matrix3 accCovariance() const { return accNoiseModel_->covariance(); } - /// Pose of body in nav frame at time t - virtual Pose3 pose(double t) const = 0; + // Quantities a Scenario needs to specify: + + virtual Pose3 pose(double t) const = 0; + virtual Vector3 omega_b(double t) const = 0; + virtual Vector3 velocity_n(double t) const = 0; + virtual Vector3 acceleration_n(double t) const = 0; + + // Derived quantities: - /// Rotation of body in nav frame at time t virtual Rot3 rotation(double t) const { return pose(t).rotation(); } - virtual Vector3 angularVelocityInBody(double t) const = 0; - virtual Vector3 linearVelocityInBody(double t) const = 0; - virtual Vector3 accelerationInBody(double t) const = 0; - - /// Velocity in nav frame at time t - Vector3 velocity(double t) const { - return rotation(t) * linearVelocityInBody(t); + Vector3 velocity_b(double t) const { + const Rot3 nRb = rotation(t); + return nRb.transpose() * velocity_n(t); } - // acceleration in nav frame - // TODO(frank): correct for rotating frames? - Vector3 acceleration(double t) const { - return rotation(t) * accelerationInBody(t); + Vector3 acceleration_b(double t) const { + const Rot3 nRb = rotation(t); + return nRb.transpose() * acceleration_n(t); } private: @@ -80,9 +80,7 @@ class Scenario { noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_; }; -/** - * Simple IMU simulator with constant twist 3D trajectory. - */ +/// Scenario with constant twist 3D trajectory. class ExpmapScenario : public Scenario { public: /// Construct scenario with constant twist [w,v] @@ -91,21 +89,40 @@ class ExpmapScenario : public Scenario { double accSigma = 0.01) : Scenario(imuSampleTime, gyroSigma, accSigma), twist_((Vector6() << w, v).finished()), - centripetalAcceleration_(twist_.head<3>().cross(twist_.tail<3>())) {} + a_b_(twist_.head<3>().cross(twist_.tail<3>())) {} Pose3 pose(double t) const { return Pose3::Expmap(twist_ * t); } - - Vector3 angularVelocityInBody(double t) const { return twist_.head<3>(); } - Vector3 linearVelocityInBody(double t) const { return twist_.tail<3>(); } - - Vector3 accelerationInBody(double t) const { - const Rot3 nRb = rotation(t); - return centripetalAcceleration_ - nRb.transpose() * gravity(); - } + Vector3 omega_b(double t) const { return twist_.head<3>(); } + Vector3 velocity_n(double t) const { return rotation(t) * twist_.tail<3>(); } + Vector3 acceleration_n(double t) const { return rotation(t) * a_b_; } private: const Vector6 twist_; - const Vector3 centripetalAcceleration_; + const Vector3 a_b_; // constant centripetal acceleration in body = w_b * v_b +}; + +/// Accelerating from an arbitrary initial state +class AcceleratingScenario : public Scenario { + public: + /// Construct scenario with constant twist [w,v] + AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0, + const Vector3& accelerationInBody, + double imuSampleTime = 1.0 / 100.0, + double gyroSigma = 0.17, double accSigma = 0.01) + : Scenario(imuSampleTime, gyroSigma, accSigma), + nRb_(nRb), + p0_(p0.vector()), + v0_(v0), + a_n_(nRb_ * accelerationInBody) {} + + Pose3 pose(double t) const { return Pose3(nRb_, p0_ + a_n_ * t * t / 2.0); } + Vector3 omega_b(double t) const { return Vector3::Zero(); } + Vector3 velocity_n(double t) const { return v0_ + a_n_ * t; } + Vector3 acceleration_n(double t) const { return a_n_; } + + private: + const Rot3 nRb_; + const Vector3 p0_, v0_, a_n_; }; } // namespace gtsam diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 048692a37..40f5f8585 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -48,9 +48,9 @@ class ScenarioRunner { const size_t nrSteps = T / dt; double t = 0; for (size_t k = 0; k < nrSteps; k++, t += dt) { - Vector3 measuredOmega = scenario_->angularVelocityInBody(t); + Vector3 measuredOmega = scenario_->omega_b(t); if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt; - Vector3 measuredAcc = scenario_->accelerationInBody(t); + Vector3 measuredAcc = scenario_->acceleration_b(t); if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt; pim.integrateMeasurement(measuredAcc, measuredOmega, dt); } @@ -64,7 +64,7 @@ class ScenarioRunner { // TODO(frank): allow non-zero bias, omegaCoriolis const imuBias::ConstantBias zeroBias; const Pose3 pose_i = Pose3::identity(); - const Vector3 vel_i = scenario_->velocity(0); + const Vector3 vel_i = scenario_->velocity_n(0); const Vector3 omegaCoriolis = Vector3::Zero(); const bool use2ndOrderCoriolis = true; return pim.predict(pose_i, vel_i, zeroBias, scenario_->gravity(), diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index ffac96902..b975440c7 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/gtsam/navigation/tests/testScenario.cpp @@ -27,9 +27,15 @@ static const double degree = M_PI / 180.0; /* ************************************************************************* */ TEST(Scenario, Forward) { const double v = 2; // m/s - ExpmapScenario forward(Vector3::Zero(), Vector3(v, 0, 0)); + const Vector3 W(0, 0, 0), V(v, 0, 0); + const ExpmapScenario scenario(W, V); - const Pose3 T15 = forward.pose(15); + const double T = 15; + EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9)); + EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9)); + EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9)); + + const Pose3 T15 = scenario.pose(T); EXPECT(assert_equal(Vector3(0, 0, 0), T15.rotation().xyz(), 1e-9)); EXPECT(assert_equal(Point3(30, 0, 0), T15.translation(), 1e-9)); } @@ -38,11 +44,17 @@ TEST(Scenario, Forward) { TEST(Scenario, Circle) { // Forward velocity 2m/s, angular velocity 6 degree/sec around Z const double v = 2, w = 6 * degree; - ExpmapScenario circle(Vector3(0, 0, w), Vector3(v, 0, 0)); + const Vector3 W(0, 0, w), V(v, 0, 0); + const ExpmapScenario scenario(W, V); + + const double T = 15; + EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9)); + EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9)); + EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9)); // R = v/w, so test if circle is of right size const double R = v / w; - const Pose3 T15 = circle.pose(15); + const Pose3 T15 = scenario.pose(T); EXPECT(assert_equal(Vector3(0, 0, 90 * degree), T15.rotation().xyz(), 1e-9)); EXPECT(assert_equal(Point3(R, R, 0), T15.translation(), 1e-9)); } @@ -52,15 +64,44 @@ TEST(Scenario, Loop) { // Forward velocity 2m/s // Pitch up with angular velocity 6 degree/sec (negative in FLU) const double v = 2, w = 6 * degree; - ExpmapScenario loop(Vector3(0, -w, 0), Vector3(v, 0, 0)); + const Vector3 W(0, -w, 0), V(v, 0, 0); + const ExpmapScenario scenario(W, V); + + const double T = 30; + EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9)); + EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9)); + EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9)); // R = v/w, so test if loop crests at 2*R const double R = v / w; - const Pose3 T30 = loop.pose(30); + const Pose3 T30 = scenario.pose(30); EXPECT(assert_equal(Vector3(-M_PI, 0, -M_PI), T30.rotation().xyz(), 1e-9)); EXPECT(assert_equal(Point3(0, 0, 2 * R), T30.translation(), 1e-9)); } +/* ************************************************************************* */ +TEST(Scenario, Accelerating) { + // Set up body pointing towards y axis, and start at 10,20,0 with velocity + // going in X. The body itself has Z axis pointing down + const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); + const Point3 P0(10, 20, 0); + const Vector3 V0(50, 0, 0); + + const double a_b = 0.2; // m/s^2 + const AcceleratingScenario scenario(nRb, P0, V0, Vector3(a_b, 0, 0)); + + const double T = 3; + const Vector3 A = nRb * Vector3(a_b, 0, 0); + EXPECT(assert_equal(Vector3(0, 0, 0), scenario.omega_b(T), 1e-9)); + EXPECT(assert_equal(Vector3(V0 + T * A), scenario.velocity_n(T), 1e-9)); + EXPECT(assert_equal(A, scenario.acceleration_n(T), 1e-9)); + + const Pose3 T3 = scenario.pose(3); + EXPECT(assert_equal(nRb, T3.rotation(), 1e-9)); + EXPECT(assert_equal(Point3(P0.vector() + T * T * A / 2.0), T3.translation(), + 1e-9)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index 3a6b63b92..15c2456b6 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -78,6 +78,30 @@ TEST(ScenarioRunner, Loop) { EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); } +/* ************************************************************************* */ +TEST(ScenarioRunner, Accelerating) { + // Set up body pointing towards y axis, and start at 10,20,0 with velocity + // going in X + // The body itself has Z axis pointing down + const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); + const Point3 initial_position(10, 20, 0); + const Vector3 initial_velocity(50, 0, 0); + + const double a = 0.2, dt = 3.0; + AcceleratingScenario scenario(nRb, initial_velocity, initial_velocity, + Vector3(a, 0, 0), dt / 10, kGyroSigma, + kAccelerometerSigma); + + ScenarioRunner runner(&scenario); + const double T = 3; // seconds + + ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); + + Matrix6 estimatedCov = runner.estimatePoseCovariance(T); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); +} + /* ************************************************************************* */ int main() { TestResult tr;