diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index 643d44f3d..136737077 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -21,13 +21,26 @@ namespace gtsam { +//////////////////////////////////////////////////////////////////////////////////////////// + +void PreintegratedMeasurements2::integrateMeasurement( + const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {} + +NavState PreintegratedMeasurements2::predict( + const NavState& state_i, const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 9> H1, OptionalJacobian<9, 6> H2) const { + return NavState(); +} + +//////////////////////////////////////////////////////////////////////////////////////////// + static double intNoiseVar = 0.0000001; static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; -ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate( +PreintegratedMeasurements2 ScenarioRunner::integrate( double T, const imuBias::ConstantBias& estimatedBias, bool corrupted) const { - ImuFactor::PreintegratedMeasurements pim(p_, estimatedBias); + PreintegratedMeasurements2 pim(p_, estimatedBias); const double dt = imuSampleTime(); const size_t nrSteps = T / dt; @@ -43,7 +56,7 @@ ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate( } NavState ScenarioRunner::predict( - const ImuFactor::PreintegratedMeasurements& pim, + const PreintegratedMeasurements2& pim, const imuBias::ConstantBias& estimatedBias) const { const NavState state_i(scenario_->pose(0), scenario_->velocity_n(0)); return pim.predict(state_i, estimatedBias); diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 33a456e87..47170a42e 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -22,14 +22,41 @@ namespace gtsam { +/** + * Class that integrates on the manifold + */ +class PreintegratedMeasurements2 { + public: + typedef ImuFactor::PreintegratedMeasurements::Params Params; + + Matrix9 preintMeasCov() const { return Matrix9::Zero(); } + + PreintegratedMeasurements2( + const boost::shared_ptr& p, + const gtsam::imuBias::ConstantBias& estimatedBias) {} + + /** + * Add a single IMU measurement to the preintegration. + * @param measuredAcc Measured acceleration (in body frame) + * @param measuredOmega Measured angular velocity (in body frame) + * @param dt Time interval between this and the last IMU measurement + */ + void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, double dt); + + /// Predict state at time j + NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 9> H1 = boost::none, + OptionalJacobian<9, 6> H2 = boost::none) const; +}; + /* * Simple class to test navigation scenarios. * Takes a trajectory scenario as input, and can generate IMU measurements */ class ScenarioRunner { public: - typedef boost::shared_ptr - SharedParams; + typedef boost::shared_ptr SharedParams; ScenarioRunner(const Scenario* scenario, const SharedParams& p, double imuSampleTime = 1.0 / 100.0, const imuBias::ConstantBias& bias = imuBias::ConstantBias()) @@ -67,19 +94,18 @@ class ScenarioRunner { const double& imuSampleTime() const { return imuSampleTime_; } /// Integrate measurements for T seconds into a PIM - ImuFactor::PreintegratedMeasurements integrate( + PreintegratedMeasurements2 integrate( double T, const imuBias::ConstantBias& estimatedBias = imuBias::ConstantBias(), bool corrupted = false) const; /// Predict predict given a PIM - NavState predict(const ImuFactor::PreintegratedMeasurements& pim, + NavState predict(const PreintegratedMeasurements2& pim, const imuBias::ConstantBias& estimatedBias = imuBias::ConstantBias()) const; /// Return pose covariance by re-arranging pim.preintMeasCov() appropriately - Matrix6 poseCovariance( - const ImuFactor::PreintegratedMeasurements& pim) const { + Matrix6 poseCovariance(const PreintegratedMeasurements2& pim) const { Matrix9 cov = pim.preintMeasCov(); Matrix6 poseCov; poseCov << cov.block<3, 3>(0, 0), cov.block<3, 3>(0, 3), // diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index b882f2597..c78afcbef 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -31,7 +31,7 @@ static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3); static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias); // Create default parameters with Z-down and above noise parameters -static boost::shared_ptr defaultParams() { +static boost::shared_ptr defaultParams() { auto p = PreintegratedImuMeasurements::Params::MakeSharedD(10); p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; @@ -50,7 +50,7 @@ TEST(ScenarioRunner, Forward) { ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); const double T = 0.1; // seconds - ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); Matrix6 estimatedCov = runner.estimatePoseCovariance(T); @@ -63,7 +63,7 @@ TEST(ScenarioRunner, ForwardWithBias) { ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); const double T = 0.1; // seconds - ImuFactor::PreintegratedMeasurements pim = runner.integrate(T, kNonZeroBias); + auto pim = runner.integrate(T, kNonZeroBias); Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 1000, kNonZeroBias); EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); } @@ -77,7 +77,7 @@ TEST(ScenarioRunner, Circle) { ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); const double T = 0.1; // seconds - ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1)); Matrix6 estimatedCov = runner.estimatePoseCovariance(T); @@ -94,7 +94,7 @@ TEST(ScenarioRunner, Loop) { ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); const double T = 0.1; // seconds - ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1)); Matrix6 estimatedCov = runner.estimatePoseCovariance(T); @@ -125,7 +125,7 @@ TEST(ScenarioRunner, Accelerating) { using namespace accelerating; ScenarioRunner runner(&scenario, defaultParams(), T / 10); - ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); Matrix6 estimatedCov = runner.estimatePoseCovariance(T); @@ -139,7 +139,7 @@ TEST(ScenarioRunner, Accelerating) { // ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma, // kNonZeroBias); // -// ImuFactor::PreintegratedMeasurements pim = runner.integrate(T, +// auto pim = runner.integrate(T, // kNonZeroBias); // Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 10000, // kNonZeroBias); @@ -156,7 +156,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating) { const double T = 3; // seconds ScenarioRunner runner(&scenario, defaultParams(), T / 10); - ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); Matrix6 estimatedCov = runner.estimatePoseCovariance(T);