From dfe3f3a34804fdbd7dc56146ccba47609d320d75 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Dec 2015 09:21:54 -0800 Subject: [PATCH] Split off Scenario abstract base class --- gtsam/navigation/Scenario.h | 67 +++++++++++-------- gtsam/navigation/ScenarioRunner.h | 20 +++--- gtsam/navigation/tests/testScenario.cpp | 6 +- gtsam/navigation/tests/testScenarioRunner.cpp | 24 +++---- 4 files changed, 65 insertions(+), 52 deletions(-) diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index 2fae5bf74..58d6057b3 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -22,7 +22,7 @@ namespace gtsam { /** - * Simple IMU simulator with constant twist 3D trajectory. + * Simple IMU simulator. * It is also assumed that gravity is magically counteracted and has no effect * on trajectory. Hence, a simulated IMU yields the actual body angular * velocity, and negative G acceleration plus the acceleration created by the @@ -31,11 +31,9 @@ namespace gtsam { class Scenario { public: /// Construct scenario with constant twist [w,v] - Scenario(const Vector3& w, const Vector3& v, - double imuSampleTime = 1.0 / 100.0, double gyroSigma = 0.17, + Scenario(double imuSampleTime = 1.0 / 100.0, double gyroSigma = 0.17, double accSigma = 0.01) - : twist_((Vector6() << w, v).finished()), - imuSampleTime_(imuSampleTime), + : imuSampleTime_(imuSampleTime), gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)), accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)) {} @@ -56,43 +54,58 @@ class Scenario { Matrix3 gyroCovariance() const { return gyroNoiseModel_->covariance(); } Matrix3 accCovariance() const { return accNoiseModel_->covariance(); } - Vector3 angularVelocityInBody() const { return twist_.head<3>(); } - Vector3 linearVelocityInBody() const { return twist_.tail<3>(); } + /// Pose of body in nav frame at time t + virtual Pose3 pose(double t) const = 0; /// Rotation of body in nav frame at time t - Rot3 rotAtTime(double t) const { - return Rot3::Expmap(angularVelocityInBody() * t); - } + virtual Rot3 rotation(double t) const { return pose(t).rotation(); } - /// Pose of body in nav frame at time t - Pose3 pose(double t) const { return Pose3::Expmap(twist_ * t); } + 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 { - const Rot3 nRb = rotAtTime(t); - return nRb * linearVelocityInBody(); + return rotation(t) * linearVelocityInBody(t); } // acceleration in nav frame + // TODO(frank): correct for rotating frames? Vector3 acceleration(double t) const { - const Vector3 centripetalAcceleration = - angularVelocityInBody().cross(linearVelocityInBody()); - const Rot3 nRb = rotAtTime(t); - return nRb * centripetalAcceleration - gravity(); - } - - // acceleration in body frame frame - Vector3 accelerationInBody(double t) const { - const Vector3 centripetalAcceleration = - angularVelocityInBody().cross(linearVelocityInBody()); - const Rot3 nRb = rotAtTime(t); - return centripetalAcceleration - nRb.transpose() * gravity(); + return rotation(t) * accelerationInBody(t); } private: - Vector6 twist_; double imuSampleTime_; noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_; }; +/** + * Simple IMU simulator with constant twist 3D trajectory. + */ +class ExpmapScenario : public Scenario { + public: + /// Construct scenario with constant twist [w,v] + ExpmapScenario(const Vector3& w, const Vector3& v, + double imuSampleTime = 1.0 / 100.0, double gyroSigma = 0.17, + double accSigma = 0.01) + : Scenario(imuSampleTime, gyroSigma, accSigma), + twist_((Vector6() << w, v).finished()), + centripetalAcceleration_(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(); + } + + private: + const Vector6 twist_; + const Vector3 centripetalAcceleration_; +}; + } // namespace gtsam diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 310d96d6f..048692a37 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -30,7 +30,7 @@ static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; /// Simple class to test navigation scenarios class ScenarioRunner { public: - ScenarioRunner(const Scenario& scenario) : scenario_(scenario) {} + ScenarioRunner(const Scenario* scenario) : scenario_(scenario) {} /// Integrate measurements for T seconds into a PIM ImuFactor::PreintegratedMeasurements integrate( @@ -40,17 +40,17 @@ class ScenarioRunner { const bool use2ndOrderIntegration = true; ImuFactor::PreintegratedMeasurements pim( - zeroBias, scenario_.accCovariance(), scenario_.gyroCovariance(), + zeroBias, scenario_->accCovariance(), scenario_->gyroCovariance(), kIntegrationErrorCovariance, use2ndOrderIntegration); - const double dt = scenario_.imuSampleTime(); + const double dt = scenario_->imuSampleTime(); const double sqrt_dt = std::sqrt(dt); const size_t nrSteps = T / dt; double t = 0; for (size_t k = 0; k < nrSteps; k++, t += dt) { - Vector3 measuredOmega = scenario_.angularVelocityInBody(); + Vector3 measuredOmega = scenario_->angularVelocityInBody(t); if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt; - Vector3 measuredAcc = scenario_.accelerationInBody(t); + Vector3 measuredAcc = scenario_->accelerationInBody(t); if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt; pim.integrateMeasurement(measuredAcc, measuredOmega, dt); } @@ -64,10 +64,10 @@ 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(0); const Vector3 omegaCoriolis = Vector3::Zero(); const bool use2ndOrderCoriolis = true; - return pim.predict(pose_i, vel_i, zeroBias, scenario_.gravity(), + return pim.predict(pose_i, vel_i, zeroBias, scenario_->gravity(), omegaCoriolis, use2ndOrderCoriolis); } @@ -87,8 +87,8 @@ class ScenarioRunner { Pose3 prediction = predict(integrate(T)).pose; // Create two samplers for acceleration and omega noise - Sampler gyroSampler(scenario_.gyroNoiseModel(), 10); - Sampler accSampler(scenario_.accNoiseModel(), 29284); + Sampler gyroSampler(scenario_->gyroNoiseModel(), 10); + Sampler accSampler(scenario_->accNoiseModel(), 29284); // Sample ! Matrix samples(9, N); @@ -114,7 +114,7 @@ class ScenarioRunner { } private: - Scenario scenario_; + const Scenario* scenario_; }; } // namespace gtsam diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index ae406f953..ffac96902 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/gtsam/navigation/tests/testScenario.cpp @@ -27,7 +27,7 @@ static const double degree = M_PI / 180.0; /* ************************************************************************* */ TEST(Scenario, Forward) { const double v = 2; // m/s - Scenario forward(Vector3::Zero(), Vector3(v, 0, 0)); + ExpmapScenario forward(Vector3::Zero(), Vector3(v, 0, 0)); const Pose3 T15 = forward.pose(15); EXPECT(assert_equal(Vector3(0, 0, 0), T15.rotation().xyz(), 1e-9)); @@ -38,7 +38,7 @@ TEST(Scenario, Forward) { TEST(Scenario, Circle) { // Forward velocity 2m/s, angular velocity 6 degree/sec around Z const double v = 2, w = 6 * degree; - Scenario circle(Vector3(0, 0, w), Vector3(v, 0, 0)); + ExpmapScenario circle(Vector3(0, 0, w), Vector3(v, 0, 0)); // R = v/w, so test if circle is of right size const double R = v / w; @@ -52,7 +52,7 @@ 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; - Scenario loop(Vector3(0, -w, 0), Vector3(v, 0, 0)); + ExpmapScenario loop(Vector3(0, -w, 0), Vector3(v, 0, 0)); // R = v/w, so test if loop crests at 2*R const double R = v / w; diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index 42bffa1a5..3a6b63b92 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -30,14 +30,14 @@ static const double kAccelerometerSigma = 0.1; /* ************************************************************************* */ TEST(ScenarioRunner, Forward) { const double v = 2; // m/s - Scenario forward(Vector3::Zero(), Vector3(v, 0, 0), kDeltaT, kGyroSigma, - kAccelerometerSigma); + ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0), kDeltaT, + kGyroSigma, kAccelerometerSigma); - ScenarioRunner runner(forward); + ScenarioRunner runner(&scenario); const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); - EXPECT(assert_equal(forward.pose(T), runner.predict(pim).pose, 1e-9)); + 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)); @@ -47,14 +47,14 @@ TEST(ScenarioRunner, Forward) { TEST(ScenarioRunner, Circle) { // Forward velocity 2m/s, angular velocity 6 kDegree/sec const double v = 2, w = 6 * kDegree; - Scenario circle(Vector3(0, 0, w), Vector3(v, 0, 0), kDeltaT, kGyroSigma, - kAccelerometerSigma); + ExpmapScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0), kDeltaT, + kGyroSigma, kAccelerometerSigma); - ScenarioRunner runner(circle); + ScenarioRunner runner(&scenario); const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); - EXPECT(assert_equal(circle.pose(T), runner.predict(pim).pose, 0.1)); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 0.1)); Matrix6 estimatedCov = runner.estimatePoseCovariance(T); EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); @@ -65,14 +65,14 @@ TEST(ScenarioRunner, Loop) { // Forward velocity 2m/s // Pitch up with angular velocity 6 kDegree/sec (negative in FLU) const double v = 2, w = 6 * kDegree; - Scenario loop(Vector3(0, -w, 0), Vector3(v, 0, 0), kDeltaT, kGyroSigma, - kAccelerometerSigma); + ExpmapScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0), kDeltaT, + kGyroSigma, kAccelerometerSigma); - ScenarioRunner runner(loop); + ScenarioRunner runner(&scenario); const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); - EXPECT(assert_equal(loop.pose(T), runner.predict(pim).pose, 0.1)); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 0.1)); Matrix6 estimatedCov = runner.estimatePoseCovariance(T); EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));