From 31335608a86bf23702a95713c019dce4c85553da Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Dec 2015 14:29:42 -0800 Subject: [PATCH 1/2] Acceleration now specified in nav frame, allow angular velocity --- gtsam/navigation/Scenario.h | 16 ++++++++------ gtsam/navigation/tests/testScenario.cpp | 22 ++++++++++++++----- gtsam/navigation/tests/testScenarioRunner.cpp | 5 +++-- 3 files changed, 28 insertions(+), 15 deletions(-) diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index 3324abaab..bc9dfe8eb 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -67,24 +67,26 @@ class ExpmapScenario : public Scenario { const Vector3 a_b_; // constant centripetal acceleration in body = w_b * v_b }; -/// Accelerating from an arbitrary initial state +/// Accelerating from an arbitrary initial state, with optional rotation class AcceleratingScenario : public Scenario { public: - /// Construct scenario with constant twist [w,v] + /// 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& accelerationInBody) - : nRb_(nRb), p0_(p0.vector()), v0_(v0), a_n_(nRb_ * accelerationInBody) {} + 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_, p0_ + v0_ * t + a_n_ * t * t / 2.0); + return Pose3(nRb_.expmap(omega_b_ * t), p0_ + v0_ * t + a_n_ * t * t / 2.0); } - Vector3 omega_b(double t) const { return Vector3::Zero(); } + 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_; + const Vector3 p0_, v0_, a_n_, omega_b_; }; } // namespace gtsam diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index c029ecc03..ab538e02a 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/gtsam/navigation/tests/testScenario.cpp @@ -16,7 +16,10 @@ */ #include +#include + #include +#include #include using namespace std; @@ -87,18 +90,25 @@ TEST(Scenario, Accelerating) { const Point3 P0(10, 20, 0); const Vector3 V0(50, 0, 0); - const double a_b = 0.2; // m/s^2 - const AcceleratingScenario scenario(nRb, P0, V0, Vector3(a_b, 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; - const Vector3 A = nRb * Vector3(a_b, 0, 0); - EXPECT(assert_equal(Vector3(0, 0, 0), scenario.omega_b(T), 1e-9)); + 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, T3.rotation(), 1e-9)); - EXPECT(assert_equal(Point3(10 + T * 50, 20 + a_b * T * T / 2, 0), + 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)); } diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index b7a864016..8d01f5572 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -83,8 +83,9 @@ TEST(ScenarioRunner, Accelerating) { const Point3 P0(10, 20, 0); const Vector3 V0(50, 0, 0); - const double a_b = 0.2; // m/s^2 - const AcceleratingScenario scenario(nRb, P0, V0, Vector3(a_b, 0, 0)); + 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 ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma); From f79a9b8d3a04a11ad0b29b3002263933c9d2c729 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Dec 2015 15:04:36 -0800 Subject: [PATCH 2/2] Make two acceleration scenarios --- gtsam/navigation/tests/testScenarioRunner.cpp | 37 +++++++++++++++---- 1 file changed, 30 insertions(+), 7 deletions(-) diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index 8d01f5572..bf3e3836a 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -76,13 +76,17 @@ 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 - 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 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); +} +/* ************************************************************************* */ +TEST(ScenarioRunner, 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); @@ -93,7 +97,26 @@ TEST(ScenarioRunner, Accelerating) { ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); - Matrix6 estimatedCov = runner.estimatePoseCovariance(T); + Matrix6 estimatedCov = runner.estimatePoseCovariance(T,10000); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); + cout << estimatedCov << endl << endl; + cout << runner.poseCovariance(pim) << endl; +} + +/* ************************************************************************* */ +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, kAccelerometerSigma); + + ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); + + Matrix6 estimatedCov = runner.estimatePoseCovariance(T,10000); EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); }