diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index e4b380ff9..acb8f46f5 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -57,10 +57,13 @@ class Scenario { class ConstantTwistScenario : public Scenario { public: /// Construct scenario with constant twist [w,v] - ConstantTwistScenario(const Vector3& w, const Vector3& v) - : twist_((Vector6() << w, v).finished()), a_b_(w.cross(v)) {} + ConstantTwistScenario(const Vector3& w, const Vector3& v, + const Pose3& nTb0 = Pose3()) + : twist_((Vector6() << w, v).finished()), a_b_(w.cross(v)), nTb0_(nTb0) {} - Pose3 pose(double t) const override { return Pose3::Expmap(twist_ * t); } + Pose3 pose(double t) const override { + return nTb0_ * Pose3::Expmap(twist_ * t); + } Vector3 omega_b(double t) const override { return twist_.head<3>(); } Vector3 velocity_n(double t) const override { return rotation(t).matrix() * twist_.tail<3>(); @@ -70,6 +73,7 @@ class ConstantTwistScenario : public Scenario { private: const Vector6 twist_; const Vector3 a_b_; // constant centripetal acceleration in body = w_b * v_b + const Pose3 nTb0_; }; /// Accelerating from an arbitrary initial state, with optional rotation diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index bc965b058..ef30552ea 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/gtsam/navigation/tests/testScenario.cpp @@ -15,8 +15,8 @@ * @author Frank Dellaert */ -#include #include +#include #include #include @@ -99,6 +99,29 @@ TEST(Scenario, Loop) { EXPECT(assert_equal(Point3(0, 0, 2 * R), T30.translation(), 1e-9)); } +/* ************************************************************************* */ +TEST(Scenario, LoopWithInitialPose) { + // Forward velocity 2m/s + // Pitch up with angular velocity 6 kDegree/sec (negative in FLU) + const double v = 2, w = 6 * kDegree; + const Vector3 W(0, -w, 0), V(v, 0, 0); + const Rot3 nRb0 = Rot3::yaw(M_PI); + const Pose3 nTb0(nRb0, Point3(1, 2, 3)); + const ConstantTwistScenario scenario(W, V, nTb0); + + 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(nRb0 * Rot3::Rodrigues(0, M_PI, 0), T30.rotation(), 1e-9)); + EXPECT(assert_equal(Point3(1, 2, 3 + 2 * R), T30.translation(), 1e-9)); +} + /* ************************************************************************* */ TEST(Scenario, Accelerating) { // Set up body pointing towards y axis, and start at 10,20,0 with velocity