diff --git a/doc/.gitignore b/doc/.gitignore index ac7af2e80..8a3139177 100644 --- a/doc/.gitignore +++ b/doc/.gitignore @@ -1 +1,3 @@ /html/ +*.lyx~ +*.bib~ diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h new file mode 100644 index 000000000..2fae5bf74 --- /dev/null +++ b/gtsam/navigation/Scenario.h @@ -0,0 +1,98 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Scenario.h + * @brief Simple class to test navigation scenarios + * @author Frank Dellaert + */ + +#pragma once +#include +#include + +namespace gtsam { + +/** + * Simple IMU simulator with constant twist 3D trajectory. + * 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. + */ +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, + double accSigma = 0.01) + : twist_((Vector6() << w, v).finished()), + 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(); } + + Vector3 angularVelocityInBody() const { return twist_.head<3>(); } + Vector3 linearVelocityInBody() const { return twist_.tail<3>(); } + + /// Rotation of body in nav frame at time t + Rot3 rotAtTime(double t) const { + return Rot3::Expmap(angularVelocityInBody() * t); + } + + /// Pose of body in nav frame at time t + Pose3 pose(double t) const { return Pose3::Expmap(twist_ * t); } + + /// Velocity in nav frame at time t + Vector3 velocity(double t) const { + const Rot3 nRb = rotAtTime(t); + return nRb * linearVelocityInBody(); + } + + // acceleration in nav frame + 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(); + } + + private: + Vector6 twist_; + double imuSampleTime_; + noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_; +}; + +} // namespace gtsam diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h new file mode 100644 index 000000000..fa07b290f --- /dev/null +++ b/gtsam/navigation/ScenarioRunner.h @@ -0,0 +1,120 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ScenarioRunner.h + * @brief Simple class to test navigation scenarios + * @author Frank Dellaert + */ + +#pragma once +#include +#include +#include + +#include + +namespace gtsam { + +static double intNoiseVar = 0.0000001; +static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; + +/// Simple class to test navigation scenarios +class ScenarioRunner { + public: + ScenarioRunner(const Scenario& scenario) : scenario_(scenario) {} + + /// Integrate measurements for T seconds into a PIM + ImuFactor::PreintegratedMeasurements integrate( + double T, Sampler* gyroSampler = 0, Sampler* accSampler = 0) const { + // TODO(frank): allow non-zero + const imuBias::ConstantBias zeroBias; + const bool use2ndOrderIntegration = true; + + ImuFactor::PreintegratedMeasurements pim( + zeroBias, scenario_.accCovariance(), scenario_.gyroCovariance(), + kIntegrationErrorCovariance, use2ndOrderIntegration); + + 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(); + if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt; + Vector3 measuredAcc = scenario_.accelerationInBody(t); + if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt; + pim.integrateMeasurement(measuredAcc, measuredOmega, dt); + } + + return pim; + } + + /// Predict predict given a PIM + PoseVelocityBias predict( + 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(0); + const Vector3 omegaCoriolis = Vector3::Zero(); + const bool use2ndOrderCoriolis = true; + return pim.predict(pose_i, vel_i, zeroBias, scenario_.gravity(), + omegaCoriolis, use2ndOrderCoriolis); + } + + /// Return pose covariance by re-arranging pim.preintMeasCov() appropriately + Matrix6 poseCovariance( + const ImuFactor::PreintegratedMeasurements& pim) const { + Matrix9 cov = pim.preintMeasCov(); // _ position rotation + Matrix6 poseCov; + poseCov << cov.block<3, 3>(6, 6), cov.block<3, 3>(6, 3), // + cov.block<3, 3>(3, 6), cov.block<3, 3>(3, 3); + return poseCov; + } + + /// Compute a Monte Carlo estimate of the PIM pose covariance using N samples + Matrix6 estimatePoseCovariance(double T, size_t N = 1000) const { + // Get predict prediction from ground truth measurements + Pose3 prediction = predict(integrate(T)).pose; + + // Create two samplers for acceleration and omega noise + Sampler gyroSampler(scenario_.gyroNoiseModel(), 10); + Sampler accSampler(scenario_.accNoiseModel(), 29284); + + // Sample ! + Matrix samples(9, N); + Vector6 sum = Vector6::Zero(); + for (size_t i = 0; i < N; i++) { + Pose3 sampled = predict(integrate(T, &gyroSampler, &accSampler)).pose; + Vector6 xi = sampled.localCoordinates(prediction); + samples.col(i) = xi; + sum += xi; + } + + // Compute MC covariance + Vector6 sampleMean = sum / N; + Matrix6 Q; + Q.setZero(); + for (size_t i = 0; i < N; i++) { + Vector6 xi = samples.col(i); + xi -= sampleMean; + Q += xi * xi.transpose(); + } + + return Q / (N - 1); + } + + private: + Scenario scenario_; +}; + +} // namespace gtsam diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp new file mode 100644 index 000000000..ae406f953 --- /dev/null +++ b/gtsam/navigation/tests/testScenario.cpp @@ -0,0 +1,69 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testScenario.cpp + * @brief Unit test Scenario class + * @author Frank Dellaert + */ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +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)); + + const Pose3 T15 = forward.pose(15); + EXPECT(assert_equal(Vector3(0, 0, 0), T15.rotation().xyz(), 1e-9)); + EXPECT(assert_equal(Point3(30, 0, 0), T15.translation(), 1e-9)); +} + +/* ************************************************************************* */ +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)); + + // R = v/w, so test if circle is of right size + const double R = v / w; + const Pose3 T15 = circle.pose(15); + EXPECT(assert_equal(Vector3(0, 0, 90 * degree), T15.rotation().xyz(), 1e-9)); + EXPECT(assert_equal(Point3(R, R, 0), T15.translation(), 1e-9)); +} + +/* ************************************************************************* */ +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)); + + // R = v/w, so test if loop crests at 2*R + const double R = v / w; + const Pose3 T30 = loop.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)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp new file mode 100644 index 000000000..517e488dd --- /dev/null +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -0,0 +1,74 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testScenarioRunner.cpp + * @brief test ImuFacor with ScenarioRunner class + * @author Frank Dellaert + */ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +static const double degree = M_PI / 180.0; + +/* ************************************************************************* */ +TEST(ScenarioRunner, Forward) { + const double v = 2; // m/s + Scenario forward(Vector3::Zero(), Vector3(v, 0, 0), 1e-2, 0.000001, 1); + + ScenarioRunner runner(forward); + const double T = 1; // seconds + + ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + EXPECT(assert_equal(forward.pose(T), runner.predict(pim).pose, 1e-9)); + + Matrix6 estimatedCov = runner.estimatePoseCovariance(T); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-9)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, Circle) { + // Forward velocity 2m/s, angular velocity 6 degree/sec + const double v = 2, w = 6 * degree; + Scenario circle(Vector3(0, 0, w), Vector3(v, 0, 0)); + + ScenarioRunner runner(circle); + const double T = 15; // seconds + + ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + EXPECT(assert_equal(circle.pose(T), runner.predict(pim).pose, 0.1)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, 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)); + + ScenarioRunner runner(loop); + const double T = 30; // seconds + + ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + EXPECT(assert_equal(loop.pose(T), runner.predict(pim).pose, 0.1)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */