diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index 69b5dfa00..421507d92 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -21,39 +21,9 @@ namespace gtsam { -/** - * 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 - * rotating body frame. - */ +/// Simple trajectory simulator. class Scenario { public: - /// Construct scenario with constant twist [w,v] - Scenario(double imuSampleTime = 1.0 / 100.0, double gyroSigma = 0.17, - double accSigma = 0.01) - : imuSampleTime_(imuSampleTime), - gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)), - accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)) {} - - const double& imuSampleTime() const { return imuSampleTime_; } - - // NOTE(frank): hardcoded for now with Z up (gravity points in negative Z) - // also, uses g=10 for easy debugging - Vector3 gravity() const { return Vector3(0, 0, -10.0); } - - const noiseModel::Diagonal::shared_ptr& gyroNoiseModel() const { - return gyroNoiseModel_; - } - - const noiseModel::Diagonal::shared_ptr& accNoiseModel() const { - return accNoiseModel_; - } - - Matrix3 gyroCovariance() const { return gyroNoiseModel_->covariance(); } - Matrix3 accCovariance() const { return accNoiseModel_->covariance(); } - // Quantities a Scenario needs to specify: virtual Pose3 pose(double t) const = 0; @@ -74,24 +44,18 @@ class Scenario { const Rot3 nRb = rotation(t); return nRb.transpose() * acceleration_n(t); } - - private: - double imuSampleTime_; - noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_; }; /// Scenario 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()), + ExpmapScenario(const Vector3& w, const Vector3& v) + : twist_((Vector6() << w, v).finished()), a_b_(twist_.head<3>().cross(twist_.tail<3>())) {} Pose3 pose(double t) const { return Pose3::Expmap(twist_ * t); } + Rot3 rotation(double t) const { return Rot3::Expmap(twist_.head<3>() * t); } 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_; } @@ -106,14 +70,8 @@ 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) {} + const Vector3& accelerationInBody) + : 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(); } diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 40f5f8585..60cc9a751 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -30,7 +30,29 @@ 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, double imuSampleTime = 1.0 / 100.0, + double gyroSigma = 0.17, double accSigma = 0.01) + : scenario_(scenario), + imuSampleTime_(imuSampleTime), + gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)), + accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)) {} + + const double& imuSampleTime() const { return imuSampleTime_; } + + // NOTE(frank): hardcoded for now with Z up (gravity points in negative Z) + // also, uses g=10 for easy debugging + Vector3 gravity_n() const { return Vector3(0, 0, -10.0); } + + const noiseModel::Diagonal::shared_ptr& gyroNoiseModel() const { + return gyroNoiseModel_; + } + + const noiseModel::Diagonal::shared_ptr& accNoiseModel() const { + return accNoiseModel_; + } + + Matrix3 gyroCovariance() const { return gyroNoiseModel_->covariance(); } + Matrix3 accCovariance() const { return accNoiseModel_->covariance(); } /// Integrate measurements for T seconds into a PIM ImuFactor::PreintegratedMeasurements integrate( @@ -40,17 +62,18 @@ class ScenarioRunner { const bool use2ndOrderIntegration = true; ImuFactor::PreintegratedMeasurements pim( - zeroBias, scenario_->accCovariance(), scenario_->gyroCovariance(), + zeroBias, accCovariance(), gyroCovariance(), kIntegrationErrorCovariance, use2ndOrderIntegration); - const double dt = scenario_->imuSampleTime(); + const double dt = 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) { + Rot3 bRn = scenario_->rotation(t).transpose(); Vector3 measuredOmega = scenario_->omega_b(t); if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt; - Vector3 measuredAcc = scenario_->acceleration_b(t); + Vector3 measuredAcc = scenario_->acceleration_b(t) - bRn * gravity_n(); if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt; pim.integrateMeasurement(measuredAcc, measuredOmega, dt); } @@ -63,12 +86,10 @@ class ScenarioRunner { const ImuFactor::PreintegratedMeasurements& pim) const { // TODO(frank): allow non-zero bias, omegaCoriolis const imuBias::ConstantBias zeroBias; - const Pose3 pose_i = Pose3::identity(); - 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(), - omegaCoriolis, use2ndOrderCoriolis); + return pim.predict(scenario_->pose(0), scenario_->velocity_n(0), zeroBias, + gravity_n(), omegaCoriolis, use2ndOrderCoriolis); } /// Return pose covariance by re-arranging pim.preintMeasCov() appropriately @@ -87,8 +108,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(gyroNoiseModel(), 10); + Sampler accSampler(accNoiseModel(), 29284); // Sample ! Matrix samples(9, N); @@ -115,6 +136,8 @@ class ScenarioRunner { private: const Scenario* scenario_; + double imuSampleTime_; + noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_; }; } // namespace gtsam diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index 15c2456b6..c7a3216a9 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -30,10 +30,9 @@ static const double kAccelerometerSigma = 0.1; /* ************************************************************************* */ TEST(ScenarioRunner, Forward) { const double v = 2; // m/s - ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0), kDeltaT, - kGyroSigma, kAccelerometerSigma); + ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0)); - ScenarioRunner runner(&scenario); + ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelerometerSigma); const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); @@ -47,10 +46,9 @@ TEST(ScenarioRunner, Forward) { TEST(ScenarioRunner, Circle) { // Forward velocity 2m/s, angular velocity 6 kDegree/sec const double v = 2, w = 6 * kDegree; - ExpmapScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0), kDeltaT, - kGyroSigma, kAccelerometerSigma); + ExpmapScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0)); - ScenarioRunner runner(&scenario); + ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelerometerSigma); const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); @@ -65,10 +63,9 @@ 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; - ExpmapScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0), kDeltaT, - kGyroSigma, kAccelerometerSigma); + ExpmapScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0)); - ScenarioRunner runner(&scenario); + ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelerometerSigma); const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); @@ -81,19 +78,16 @@ TEST(ScenarioRunner, Loop) { /* ************************************************************************* */ 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 + // 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 Point3 P0(10, 20, 0); + const Vector3 V0(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); + const double a_b = 0.2; // m/s^2 + const AcceleratingScenario scenario(nRb, P0, V0, Vector3(a_b, 0, 0)); - ScenarioRunner runner(&scenario); const double T = 3; // seconds + ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma); ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9));