diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 65bc25060..99da0182c 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -298,7 +298,7 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, const Vector3& n_gravity, const Vector3& omegaCoriolis, - const bool use2ndOrderCoriolis) { + const bool use2ndOrderCoriolis) const { // NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility boost::shared_ptr q = boost::make_shared(p()); q->n_gravity = n_gravity; diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 856ba54cb..993c40ca7 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -100,8 +100,8 @@ public: protected: - /// Parameters - boost::shared_ptr p_; + /// Parameters. Declared mutable only for deprecated predict method. + mutable boost::shared_ptr p_; /// Acceleration and gyro bias used for preintegration imuBias::ConstantBias biasHat_; @@ -283,7 +283,7 @@ public: /// @deprecated predict PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, const Vector3& n_gravity, - const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false) const; /// @} diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h new file mode 100644 index 000000000..bc9dfe8eb --- /dev/null +++ b/gtsam/navigation/Scenario.h @@ -0,0 +1,92 @@ +/* ---------------------------------------------------------------------------- + + * 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 trajectory simulator. +class Scenario { + public: + // 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: + + virtual Rot3 rotation(double t) const { return pose(t).rotation(); } + + Vector3 velocity_b(double t) const { + const Rot3 nRb = rotation(t); + return nRb.transpose() * velocity_n(t); + } + + Vector3 acceleration_b(double t) const { + const Rot3 nRb = rotation(t); + return nRb.transpose() * acceleration_n(t); + } +}; + +/// 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) + : 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).matrix() * twist_.tail<3>(); + } + Vector3 acceleration_n(double t) const { return rotation(t) * a_b_; } + + private: + const Vector6 twist_; + const Vector3 a_b_; // constant centripetal acceleration in body = w_b * v_b +}; + +/// Accelerating from an arbitrary initial state, with optional rotation +class AcceleratingScenario : public Scenario { + public: + /// Construct scenario with constant acceleration in navigation frame and + /// optional angular velocity in body: rotating while translating... + AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0, + const Vector3& a_n, + const Vector3& omega_b = Vector3::Zero()) + : nRb_(nRb), p0_(p0.vector()), v0_(v0), a_n_(a_n), omega_b_(omega_b) {} + + Pose3 pose(double t) const { + return Pose3(nRb_.expmap(omega_b_ * t), p0_ + v0_ * t + a_n_ * t * t / 2.0); + } + Vector3 omega_b(double t) const { return omega_b_; } + 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_, omega_b_; +}; + +} // namespace gtsam diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp new file mode 100644 index 000000000..3b0a763d8 --- /dev/null +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -0,0 +1,88 @@ +/* ---------------------------------------------------------------------------- + + * 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 + */ + +#include + +#include + +namespace gtsam { + +static double intNoiseVar = 0.0000001; +static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; + +ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate( + double T, const imuBias::ConstantBias& estimatedBias, + bool corrupted) const { + const bool use2ndOrderIntegration = true; + + ImuFactor::PreintegratedMeasurements pim( + estimatedBias, accCovariance(), gyroCovariance(), + kIntegrationErrorCovariance, use2ndOrderIntegration); + + const double dt = imuSampleTime(); + const size_t nrSteps = T / dt; + double t = 0; + for (size_t k = 0; k < nrSteps; k++, t += dt) { + Vector3 measuredOmega = corrupted ? measured_omega_b(t) : actual_omega_b(t); + Vector3 measuredAcc = + corrupted ? measured_specific_force_b(t) : actual_specific_force_b(t); + pim.integrateMeasurement(measuredAcc, measuredOmega, dt); + } + + return pim; +} + +PoseVelocityBias ScenarioRunner::predict( + const ImuFactor::PreintegratedMeasurements& pim, + const imuBias::ConstantBias& estimatedBias) const { + // TODO(frank): allow non-zero omegaCoriolis + const Vector3 omegaCoriolis = Vector3::Zero(); + const bool use2ndOrderCoriolis = true; + return pim.predict(scenario_->pose(0), scenario_->velocity_n(0), + estimatedBias, gravity_n(), omegaCoriolis, + use2ndOrderCoriolis); +} + +Matrix6 ScenarioRunner::estimatePoseCovariance( + double T, size_t N, const imuBias::ConstantBias& estimatedBias) const { + // Get predict prediction from ground truth measurements + Pose3 prediction = predict(integrate(T)).pose; + + // Sample ! + Matrix samples(9, N); + Vector6 sum = Vector6::Zero(); + for (size_t i = 0; i < N; i++) { + Pose3 sampled = predict(integrate(T, estimatedBias, true)).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); +} + +} // namespace gtsam diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h new file mode 100644 index 000000000..d32d7b836 --- /dev/null +++ b/gtsam/navigation/ScenarioRunner.h @@ -0,0 +1,117 @@ +/* ---------------------------------------------------------------------------- + + * 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 + +namespace gtsam { + +/* + * Simple class to test navigation scenarios. + * Takes a trajectory scenario as input, and can generate IMU measurements + */ +class ScenarioRunner { + public: + ScenarioRunner(const Scenario* scenario, double imuSampleTime = 1.0 / 100.0, + double gyroSigma = 0.17, double accSigma = 0.01, + const imuBias::ConstantBias& bias = imuBias::ConstantBias()) + : scenario_(scenario), + imuSampleTime_(imuSampleTime), + gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)), + accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)), + bias_(bias), + // NOTE(duy): random seeds that work well: + gyroSampler_(gyroNoiseModel_, 10), + accSampler_(accNoiseModel_, 29284) + + {} + + // NOTE(frank): hardcoded for now with Z up (gravity points in negative Z) + // also, uses g=10 for easy debugging + static Vector3 gravity_n() { return Vector3(0, 0, -10.0); } + + // A gyro simply measures angular velocity in body frame + Vector3 actual_omega_b(double t) const { return scenario_->omega_b(t); } + + // An accelerometer measures acceleration in body, but not gravity + Vector3 actual_specific_force_b(double t) const { + Rot3 bRn = scenario_->rotation(t).transpose(); + return scenario_->acceleration_b(t) - bRn * gravity_n(); + } + + // versions corrupted by bias and noise + Vector3 measured_omega_b(double t) const { + return actual_omega_b(t) + bias_.gyroscope() + + gyroSampler_.sample() / std::sqrt(imuSampleTime_); + } + Vector3 measured_specific_force_b(double t) const { + return actual_specific_force_b(t) + bias_.accelerometer() + + accSampler_.sample() / std::sqrt(imuSampleTime_); + } + + const double& imuSampleTime() const { return imuSampleTime_; } + + 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( + double T, + const imuBias::ConstantBias& estimatedBias = imuBias::ConstantBias(), + bool corrupted = false) const; + + /// Predict predict given a PIM + PoseVelocityBias predict(const ImuFactor::PreintegratedMeasurements& pim, + const imuBias::ConstantBias& estimatedBias = + imuBias::ConstantBias()) const; + + /// Return pose covariance by re-arranging pim.preintMeasCov() appropriately + Matrix6 poseCovariance( + const ImuFactor::PreintegratedMeasurements& pim) const { + Matrix9 cov = pim.preintMeasCov(); + Matrix6 poseCov; + poseCov << cov.block<3, 3>(0, 0), cov.block<3, 3>(0, 3), // + cov.block<3, 3>(3, 0), 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 imuBias::ConstantBias& estimatedBias = + imuBias::ConstantBias()) const; + + private: + const Scenario* scenario_; + const double imuSampleTime_; + const noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_; + const imuBias::ConstantBias bias_; + + // Create two samplers for acceleration and omega noise + mutable Sampler gyroSampler_, accSampler_; +}; + +} // namespace gtsam diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 7e574ad0c..2e53bd16f 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -56,15 +57,17 @@ Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i, // Define covariance matrices /* ************************************************************************* */ -double accNoiseVar = 0.01; -double omegaNoiseVar = 0.03; -double intNoiseVar = 0.0001; -const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3; -const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * I_3x3; -const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; -const Vector3 accNoiseVar2(0.01, 0.02, 0.03); -const Vector3 omegaNoiseVar2(0.03, 0.01, 0.02); -int32_t accSamplerSeed = 29284, omegaSamplerSeed = 10; +static const double kGyroSigma = 0.02; +static const double kAccelerometerSigma = 0.1; +static double omegaNoiseVar = kGyroSigma * kGyroSigma; +static double accNoiseVar = kAccelerometerSigma * kAccelerometerSigma; +static double intNoiseVar = 0.0001; +static const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3; +static const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * I_3x3; +static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; +static const Vector3 accNoiseVar2(0.01, 0.02, 0.03); +static const Vector3 omegaNoiseVar2(0.03, 0.01, 0.02); +static int32_t accSamplerSeed = 29284, omegaSamplerSeed = 10; // Auxiliary functions to test preintegrated Jacobians // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ @@ -118,114 +121,39 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) { } // namespace /* ************************************************************************* */ -bool MonteCarlo(const PreintegratedImuMeasurements& pim, - const NavState& state, const imuBias::ConstantBias& bias, double dt, - boost::optional body_P_sensor, const Vector3& measuredAcc, - const Vector3& measuredOmega, const Vector3& accNoiseVar, - const Vector3& omegaNoiseVar, size_t N = 10000, - size_t M = 1) { - // Get mean prediction from "ground truth" measurements - PreintegratedImuMeasurements pim1 = pim; - for (size_t k = 0; k < M; k++) - pim1.integrateMeasurement(measuredAcc, measuredOmega, dt, body_P_sensor); - NavState prediction = pim1.predict(state, bias); - Matrix9 actual = pim1.preintMeasCov(); - - // Do a Monte Carlo analysis to determine empirical density on the predicted state - Sampler sampleAccelerationNoise((accNoiseVar/dt).cwiseSqrt(), accSamplerSeed); - Sampler sampleOmegaNoise((omegaNoiseVar/dt).cwiseSqrt(), omegaSamplerSeed); - Matrix samples(9, N); - Vector9 sum = Vector9::Zero(); - for (size_t i = 0; i < N; i++) { - PreintegratedImuMeasurements pim2 = pim; - for (size_t k = 0; k < M; k++) { - Vector3 perturbedAcc = measuredAcc + sampleAccelerationNoise.sample(); - Vector3 perturbedOmega = measuredOmega + sampleOmegaNoise.sample(); - pim2.integrateMeasurement(perturbedAcc, perturbedOmega, dt, - body_P_sensor); - } - NavState sampled = pim2.predict(state, bias); - Vector9 xi = sampled.localCoordinates(prediction); - samples.col(i) = xi; - sum += xi; - } - - Vector9 sampleMean = sum / N; - Matrix9 Q; - Q.setZero(); - for (size_t i = 0; i < N; i++) { - Vector9 xi = samples.col(i); - xi -= sampleMean; - Q += xi * (xi.transpose() / (N - 1)); - } - - // Compare MonteCarlo value with actual (computed) value - return assert_equal(Matrix(Q), actual, 1e-4); -} - -/* ************************************************************************* */ -TEST(ImuFactor, StraightLine) { - // Set up IMU measurements - static const double g = 10; // make gravity 10 to make this easy to check - static const double v = 50.0, a = 0.2, dt = 3.0; - const double dt22 = dt * dt / 2; +TEST(ImuFactor, Accelerating) { + const double a = 0.2, v=50; // 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 - static const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); - static const Point3 initial_position(10, 20, 0); - static const Vector3 initial_velocity(v, 0, 0); - static const NavState state1(nRb, initial_position, initial_velocity); + 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(v, 0, 0); - // set up acceleration in X direction, no angular velocity. - // Since body Z-axis is pointing down, accelerometer measures table exerting force in negative Z - Vector3 measuredAcc(a, 0, -g), measuredOmega(0, 0, 0); + const AcceleratingScenario scenario(nRb, initial_position, initial_velocity, + Vector3(a, 0, 0)); - // Create parameters assuming nav frame has Z up - boost::shared_ptr p = - PreintegratedImuMeasurements::Params::MakeSharedU(g); - p->accelerometerCovariance = kMeasuredAccCovariance; - p->gyroscopeCovariance = kMeasuredOmegaCovariance; + const double T = 3.0; // 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)); + + Matrix6 estimatedCov = runner.estimatePoseCovariance(T); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); // Check G1 and G2 derivatives of pim.update - - // Now, preintegrate for 3 seconds, in 10 steps - PreintegratedImuMeasurements pim(p, kZeroBiasHat); - for (size_t i = 0; i < 10; i++) - pim.integrateMeasurement(measuredAcc, measuredOmega, dt / 10); - Matrix93 aG1,aG2; boost::function f = - boost::bind(&PreintegrationBase::updatedDeltaXij, pim, _1, _2, dt/10, + boost::bind(&PreintegrationBase::updatedDeltaXij, pim, _1, _2, T/10, boost::none, boost::none, boost::none); - pim.updatedDeltaXij(measuredAcc, measuredOmega, dt / 10, boost::none, aG1, aG2); - EXPECT(assert_equal(numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7), aG1, 1e-7)); - EXPECT(assert_equal(numericalDerivative22(f, measuredAcc, measuredOmega, 1e-7), aG2, 1e-7)); - - // Do Monte-Carlo analysis - PreintegratedImuMeasurements pimMC(kZeroBiasHat, p->accelerometerCovariance, - p->gyroscopeCovariance, Z_3x3, true); // MonteCarlo does not sample integration noise - EXPECT( - MonteCarlo(pimMC, state1, kZeroBias, dt / 10, boost::none, measuredAcc, - measuredOmega, Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar), 100000)); - - // Check integrated quantities in body frame: gravity measured by IMU is integrated! - Vector3 b_deltaP(a * dt22, 0, -g * dt22); - Vector3 b_deltaV(a * dt, 0, -g * dt); - EXPECT(assert_equal(Rot3(), pim.deltaRij())); - EXPECT(assert_equal(b_deltaP, pim.deltaPij())); - EXPECT(assert_equal(b_deltaV, pim.deltaVij())); - - // Check bias-corrected delta: also in body frame - Vector9 expectedBC; - expectedBC << Vector3(0, 0, 0), b_deltaP, b_deltaV; - EXPECT(assert_equal(expectedBC, pim.biasCorrectedDelta(kZeroBias))); - - // Check prediction in nav, note we move along x in body, along y in nav - Point3 expected_position(10 + v * dt, 20 + a * dt22, 0); - Velocity3 expected_velocity(v, a * dt, 0); - NavState expected(nRb, expected_position, expected_velocity); - EXPECT(assert_equal(expected, pim.predict(state1, kZeroBias))); + const Vector3 measuredAcc = runner.measured_acceleration(T); + const Vector3 measuredOmega = runner.measured_angular_velocity(T); + pim.updatedDeltaXij(measuredAcc, measuredOmega, T / 10, boost::none, aG1, aG2); + EXPECT(assert_equal( + numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7), aG1, 1e-7)); + EXPECT(assert_equal( + numericalDerivative22(f, measuredAcc, measuredOmega, 1e-7), aG2, 1e-7)); } /* ************************************************************************* */ @@ -615,12 +543,25 @@ Vector3 correctedAcc(const PreintegratedImuMeasurements& pim, const Vector3& mea } TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { - imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) - Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0)); - Vector3 v1(Vector3(0.5, 0.0, 0.0)); - Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)), - Point3(5.5, 1.0, -50.0)); - Vector3 v2(Vector3(0.5, 0.0, 0.0)); + const Rot3 nRb = Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)); + const Point3 initial_position(5.0, 1.0, -50.0); + const Vector3 initial_velocity(0.5, 0.0, 0.0); + + const double a = 0.2; + const AcceleratingScenario scenario(nRb, initial_position, initial_velocity, + Vector3(a, 0, 0)); + + const double T = 3.0; // 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)); + + Matrix6 estimatedCov = runner.estimatePoseCovariance(T); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); + + /////////////////////////////////////////////////////////////////////////////////////////// + Pose3 x1(nRb, initial_position); // Measurements Vector3 measuredOmega; @@ -659,42 +600,8 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { 1e-6); EXPECT(assert_equal(expectedG2, G2, 1e-5)); -#if 0 - /* - * This code is to verify the quality of the generated samples - * by checking if the covariance of the generated noises matches - * with the input covariance, and visualizing the nonlinearity of - * the sample set using the following matlab script: - * - noises = dlmread('noises.txt'); - cov(noises) - - samples = dlmread('noises.txt'); - figure(1); - for i=1:9 - subplot(3,3,i) - hist(samples(:,i), 500) - end - */ - size_t N = 100000; - Matrix samples(9,N); - Sampler sampleAccelerationNoise((accNoiseVar2/dt).cwiseSqrt(), 29284); - Sampler sampleOmegaNoise((omegaNoiseVar2/dt).cwiseSqrt(), 10); - ofstream samplesOut("preintSamples.txt"); - ofstream noiseOut("noises.txt"); - for (size_t i = 0; i +#include + +#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 + const Vector3 W(0, 0, 0), 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)); + + 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)); +} + +/* ************************************************************************* */ +TEST(Scenario, Circle) { + // Forward velocity 2m/s, angular velocity 6 degree/sec around Z + const double v = 2, w = 6 * degree; + 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 = 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)); +} + +/* ************************************************************************* */ +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; + 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 = 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 = 0.2; // m/s^2 + const Vector3 A(0, a, 0), W(0.1, 0.2, 0.3); + const AcceleratingScenario scenario(nRb, P0, V0, A, W); + + const double T = 3; + EXPECT(assert_equal(W, 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)); + + { + // Check acceleration in nav + Matrix expected = numericalDerivative11( + boost::bind(&Scenario::velocity_n, scenario, _1), T); + EXPECT(assert_equal(Vector3(expected), scenario.acceleration_n(T), 1e-9)); + } + + const Pose3 T3 = scenario.pose(3); + EXPECT(assert_equal(nRb.expmap(T * W), T3.rotation(), 1e-9)); + EXPECT(assert_equal(Point3(10 + T * 50, 20 + a * T * T / 2, 0), + T3.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..019d60f91 --- /dev/null +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -0,0 +1,159 @@ +/* ---------------------------------------------------------------------------- + + * 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 kDegree = M_PI / 180.0; +static const double kDeltaT = 1e-2; +static const double kGyroSigma = 0.02; +static const double kAccelSigma = 0.1; + +static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3); +static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias); + +/* ************************************************************************* */ +namespace forward { +const double v = 2; // m/s +ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0)); +} +/* ************************************************************************* */ +TEST(ScenarioRunner, Forward) { + using namespace forward; + ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma); + const double T = 0.1; // 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)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, ForwardWithBias) { + using namespace forward; + ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma); + const double T = 0.1; // seconds + + ImuFactor::PreintegratedMeasurements pim = runner.integrate(T, kNonZeroBias); + Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 1000, kNonZeroBias); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); +} + +/* ************************************************************************* */ +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)); + + ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma); + const double T = 0.1; // seconds + + ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + 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)); +} + +/* ************************************************************************* */ +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)); + + ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma); + const double T = 0.1; // seconds + + ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + 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)); +} + +/* ************************************************************************* */ +namespace initial { +// 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); +} + +/* ************************************************************************* */ +namespace accelerating { +using namespace initial; +const double a = 0.2; // m/s^2 +const Vector3 A(0, a, 0); +const AcceleratingScenario scenario(nRb, P0, V0, A); + +const double T = 3; // seconds +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, Accelerating) { + using namespace accelerating; + ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma); + + 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), 0.1)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, AcceleratingWithBias) { + using namespace accelerating; + ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma, + kNonZeroBias); + + ImuFactor::PreintegratedMeasurements pim = runner.integrate(T, kNonZeroBias); + Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 1000, kNonZeroBias); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, AcceleratingAndRotating) { + using namespace initial; + const double a = 0.2; // m/s^2 + const Vector3 A(0, a, 0), W(0, 0.1, 0); + const AcceleratingScenario scenario(nRb, P0, V0, A, W); + + const double T = 3; // seconds + ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma); + + 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), 0.1)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */