diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index 25c3dfdc8..ae406f953 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/gtsam/navigation/tests/testScenario.cpp @@ -36,17 +36,31 @@ TEST(Scenario, Forward) { /* ************************************************************************* */ TEST(Scenario, Circle) { - // Forward velocity 2m/s, angular velocity 6 degree/sec - const double v = 2, omega = 6 * degree; - Scenario circle(Vector3(0, 0, omega), Vector3(v, 0, 0)); + // Forward velocity 2m/s, angular velocity 6 degree/sec around Z + const double v = 2, w = 6 * degree; + Scenario circle(Vector3(0, 0, w), Vector3(v, 0, 0)); - // R = v/omega, so test if circle is of right size - const double R = v / omega; + // R = v/w, so test if circle is of right size + const double R = v / w; const Pose3 T15 = circle.pose(15); 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; + Scenario loop(Vector3(0, -w, 0), Vector3(v, 0, 0)); + + // R = v/w, so test if loop crests at 2*R + const double R = v / w; + const Pose3 T30 = loop.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)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index e7502778e..df5ebbfd4 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -106,7 +106,7 @@ TEST(ScenarioRunner, Forward) { TEST(ScenarioRunner, Circle) { // Forward velocity 2m/s, angular velocity 6 degree/sec const double v = 2, omega = 6 * degree; - Scenario circle(Vector3(0, 0, omega), Vector3(v, 0, 0), 0.01); + Scenario circle(Vector3(0, 0, omega), Vector3(v, 0, 0)); ScenarioRunner runner(circle); const double T = 15; // seconds @@ -114,6 +114,19 @@ TEST(ScenarioRunner, Circle) { EXPECT(assert_equal(circle.pose(T), runner.mean(integrated), 0.1)); } +/* ************************************************************************* */ +TEST(ScenarioRunner, Loop) { + // Forward velocity 2m/s + // Pitch up with angular velocity 6 degree/sec (negative in FLU) + const double v = 2, w = 6 * degree; + Scenario loop(Vector3(0, -w, 0), Vector3(v, 0, 0)); + + ScenarioRunner runner(loop); + const double T = 30; // seconds + ImuFactor::PreintegratedMeasurements integrated = runner.integrate(T); + EXPECT(assert_equal(loop.pose(T), runner.mean(integrated), 0.1)); +} + /* ************************************************************************* */ int main() { TestResult tr;