From 1d214d4529488b3e003b77b6529c0836c9d6128c Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 16 Oct 2018 17:01:47 -0400
Subject: [PATCH] Successful wrap of Scenario

---
 cython/gtsam/tests/test_Scenario.py     | 36 +++++++++++++++++++++++++
 gtsam.h                                 | 29 +++++++++++---------
 gtsam/navigation/tests/testScenario.cpp |  1 +
 3 files changed, 54 insertions(+), 12 deletions(-)
 create mode 100644 cython/gtsam/tests/test_Scenario.py

diff --git a/cython/gtsam/tests/test_Scenario.py b/cython/gtsam/tests/test_Scenario.py
new file mode 100644
index 000000000..4cca1400b
--- /dev/null
+++ b/cython/gtsam/tests/test_Scenario.py
@@ -0,0 +1,36 @@
+import math
+import unittest
+import numpy as np
+
+import gtsam
+
+
+class TestScenario(unittest.TestCase):
+    def setUp(self):
+        pass
+
+    def test_loop(self):
+        # Forward velocity 2m/s
+        # Pitch up with angular velocity 6 degree/sec (negative in FLU)
+        v = 2
+        w = math.radians(6)
+        W = np.array([0, -w, 0])
+        V = np.array([v, 0, 0])
+        scenario = gtsam.ConstantTwistScenario(W, V)
+
+        T = 30
+        np.testing.assert_almost_equal(W, scenario.omega_b(T))
+        np.testing.assert_almost_equal(V, scenario.velocity_b(T))
+        np.testing.assert_almost_equal(
+            np.cross(W, V), scenario.acceleration_b(T))
+
+        # R = v/w, so test if loop crests at 2*R
+        R = v / w
+        T30 = scenario.pose(T)
+        np.testing.assert_almost_equal(
+            np.array([math.pi, 0, math.pi]), T30.rotation().xyz())
+        self.assert_(gtsam.Point3(0, 0, 2 * R).equals(T30.translation(), 1e-9))
+
+
+if __name__ == '__main__':
+    unittest.main()
diff --git a/gtsam.h b/gtsam.h
index 6e828486f..bce98b35f 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -2665,10 +2665,12 @@ virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{
   gtsam::Unit3 bRef() const;
 };
 
-virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{
-  Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model,
-      const gtsam::Unit3& bRef);
-  Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model);
+virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor {
+  Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ,
+                      const gtsam::noiseModel::Diagonal* model,
+                      const gtsam::Unit3& bRef);
+  Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ,
+                      const gtsam::noiseModel::Diagonal* model);
   Pose3AttitudeFactor();
   void print(string s) const;
   bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
@@ -2677,24 +2679,27 @@ virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{
 };
 
 #include <gtsam/navigation/Scenario.h>
-virtual class Scenario {};
-
-virtual class ConstantTwistScenario : gtsam::Scenario {
-  ConstantTwistScenario(const Vector& w, const Vector& v);
+virtual class Scenario {
   gtsam::Pose3 pose(double t) const;
   Vector omega_b(double t) const;
   Vector velocity_n(double t) const;
   Vector acceleration_n(double t) const;
+  gtsam::Rot3 rotation(double t) const;
+  gtsam::NavState navState(double t) const;
+  Vector velocity_b(double t) const;
+  Vector acceleration_b(double t) const;
+};
+
+virtual class ConstantTwistScenario : gtsam::Scenario {
+  ConstantTwistScenario(const Vector& w, const Vector& v);
+  ConstantTwistScenario(const Vector& w, const Vector& v,
+                        const Pose3& nTb0);
 };
 
 virtual class AcceleratingScenario : gtsam::Scenario {
   AcceleratingScenario(const gtsam::Rot3& nRb, const gtsam::Point3& p0,
                        const Vector& v0, const Vector& a_n,
                        const Vector& omega_b);
-  gtsam::Pose3 pose(double t) const;
-  Vector omega_b(double t) const;
-  Vector velocity_n(double t) const;
-  Vector acceleration_n(double t) const;
 };
 
 //*************************************************************************
diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp
index ef30552ea..161b8841a 100644
--- a/gtsam/navigation/tests/testScenario.cpp
+++ b/gtsam/navigation/tests/testScenario.cpp
@@ -96,6 +96,7 @@ TEST(Scenario, Loop) {
   const double R = v / w;
   const Pose3 T30 = scenario.pose(30);
   EXPECT(assert_equal(Rot3::Rodrigues(0, M_PI, 0), T30.rotation(), 1e-9));
+  EXPECT(assert_equal(Vector3(M_PI, 0, M_PI), T30.rotation().xyz()));
   EXPECT(assert_equal(Point3(0, 0, 2 * R), T30.translation(), 1e-9));
 }