From ccef2faa95a345c79f3f988c9b585c6c4a9274be Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Dec 2015 11:58:41 -0800 Subject: [PATCH] Fixed pose of accelerating trajectory --- gtsam/navigation/Scenario.h | 4 +++- gtsam/navigation/tests/testScenario.cpp | 4 ++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index 421507d92..7badd6d4e 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -73,7 +73,9 @@ class AcceleratingScenario : public Scenario { 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); } + Pose3 pose(double t) const { + return Pose3(nRb_, p0_ + v0_ * t + a_n_ * t * t / 2.0); + } Vector3 omega_b(double t) const { return Vector3::Zero(); } Vector3 velocity_n(double t) const { return v0_ + a_n_ * t; } Vector3 acceleration_n(double t) const { return a_n_; } diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index b975440c7..c029ecc03 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/gtsam/navigation/tests/testScenario.cpp @@ -98,8 +98,8 @@ TEST(Scenario, Accelerating) { const Pose3 T3 = scenario.pose(3); EXPECT(assert_equal(nRb, T3.rotation(), 1e-9)); - EXPECT(assert_equal(Point3(P0.vector() + T * T * A / 2.0), T3.translation(), - 1e-9)); + EXPECT(assert_equal(Point3(10 + T * 50, 20 + a_b * T * T / 2, 0), + T3.translation(), 1e-9)); } /* ************************************************************************* */