From 846a7774915f9208bf2651f4157211805d132fa6 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <frank@skyd.io>
Date: Mon, 21 Dec 2015 15:11:48 -0800
Subject: [PATCH] Forward scenario

---
 gtsam/navigation/Scenario.h                   | 20 ++++++++++++++--
 gtsam/navigation/tests/testScenario.cpp       | 10 ++++++++
 gtsam/navigation/tests/testScenarioRunner.cpp | 23 ++++++++++++++-----
 3 files changed, 45 insertions(+), 8 deletions(-)

diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h
index 4b25d827d..7731e33df 100644
--- a/gtsam/navigation/Scenario.h
+++ b/gtsam/navigation/Scenario.h
@@ -27,12 +27,28 @@ class Scenario {
   Scenario(const Vector3& w, const Vector3& v, double imuSampleTime = 1e-2)
       : twist_((Vector6() << w, v).finished()), imuSampleTime_(imuSampleTime) {}
 
-  Vector3 groundTruthAcc() const { return twist_.tail<3>(); }
-  Vector3 groundTruthGyro() const { return twist_.head<3>(); }
+  // NOTE(frank): hardcoded for now with Z up (gravity points in negative Z)
+  // also, uses g=10 for easy debugging
+  Vector3 gravity() const { return Vector3(0, 0, -10.0); }
+
+  Vector3 groundTruthGyroInBody() const { return twist_.head<3>(); }
+  Vector3 groundTruthVelocityInBody() const { return twist_.tail<3>(); }
+
+  // All constant twist scenarios have zero acceleration
+  Vector3 groundTruthAccInBody() const { return Vector3::Zero(); }
+
   const double& imuSampleTime() const { return imuSampleTime_; }
 
+  /// Pose of body in nav frame at time t
   Pose3 poseAtTime(double t) { return Pose3::Expmap(twist_ * t); }
 
+  /// Velocity in nav frame at time t
+  Vector3 velocityAtTime(double t) {
+    const Pose3 pose = poseAtTime(t);
+    const Rot3& nRb = pose.rotation();
+    return nRb * groundTruthVelocityInBody();
+  }
+
  private:
   Vector6 twist_;
   double imuSampleTime_;
diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp
index b5461fbdc..a4176be12 100644
--- a/gtsam/navigation/tests/testScenario.cpp
+++ b/gtsam/navigation/tests/testScenario.cpp
@@ -24,6 +24,16 @@ using namespace gtsam;
 
 static const double degree = M_PI / 180.0;
 
+/* ************************************************************************* */
+TEST(Scenario, Forward) {
+  const double v = 2;  // m/s
+  Scenario forward(Vector3::Zero(), Vector3(v, 0, 0));
+
+  const Pose3 T15 = forward.poseAtTime(15);
+  EXPECT(assert_equal(Vector3(0, 0, 0), T15.rotation().xyz(), 1e-9));
+  EXPECT(assert_equal(Point3(30, 0, 0), T15.translation(), 1e-9));
+}
+
 /* ************************************************************************* */
 TEST(Scenario, Circle) {
   // Forward velocity 2m/s, angular velocity 6 degree/sec
diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp
index 00762793b..cd1bc2e35 100644
--- a/gtsam/navigation/tests/testScenarioRunner.cpp
+++ b/gtsam/navigation/tests/testScenarioRunner.cpp
@@ -42,8 +42,8 @@ class ScenarioRunner {
         zeroBias, kMeasuredAccCovariance, kMeasuredOmegaCovariance,
         kIntegrationErrorCovariance, use2ndOrderCoriolis);
 
-    const Vector3 measuredAcc = scenario_.groundTruthAcc();
-    const Vector3 measuredOmega = scenario_.groundTruthGyro();
+    const Vector3 measuredAcc = scenario_.groundTruthAccInBody();
+    const Vector3 measuredOmega = scenario_.groundTruthGyroInBody();
     double deltaT = scenario_.imuSampleTime();
     for (double t = 0; t <= T; t += deltaT) {
       result.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
@@ -57,12 +57,12 @@ class ScenarioRunner {
     // TODO(frank): allow non-standard
     const imuBias::ConstantBias zeroBias;
     const Pose3 pose_i = Pose3::identity();
-    const Vector3 vel_i = Vector3::Zero();
-    const Vector3 gravity(0, 0, 9.81);
+    const Vector3 vel_i = scenario_.velocityAtTime(0);
     const Vector3 omegaCoriolis = Vector3::Zero();
     const bool use2ndOrderCoriolis = true;
-    const PoseVelocityBias prediction = integrated.predict(
-        pose_i, vel_i, zeroBias, gravity, omegaCoriolis, use2ndOrderCoriolis);
+    const PoseVelocityBias prediction =
+        integrated.predict(pose_i, vel_i, zeroBias, scenario_.gravity(),
+                           omegaCoriolis, use2ndOrderCoriolis);
     return prediction.pose;
   }
 
@@ -87,6 +87,17 @@ using namespace gtsam;
 
 static const double degree = M_PI / 180.0;
 
+/* ************************************************************************* */
+TEST(ScenarioRunner, Forward) {
+  const double v = 2;  // m/s
+  Scenario forward(Vector3::Zero(), Vector3(v, 0, 0));
+
+  ScenarioRunner runner(forward);
+  const double T = 10;  // seconds
+  ImuFactor::PreintegratedMeasurements integrated = runner.integrate(T);
+  EXPECT(assert_equal(forward.poseAtTime(T), runner.mean(integrated), 1e-9));
+}
+
 /* ************************************************************************* */
 TEST(ScenarioRunner, Circle) {
   // Forward velocity 2m/s, angular velocity 6 degree/sec