From 8e54e003480414fa7778a0eec3302e9b5338ebac Mon Sep 17 00:00:00 2001
From: Frank <frank@skyd.io>
Date: Tue, 26 Jan 2016 16:48:04 -0800
Subject: [PATCH] ScenarioRunner wrapped and tested

---
 python/gtsam_tests/testScenarioRunner.py   | 30 ++++++++++++++++
 python/handwritten/navigation/Scenario.cpp | 41 +++++++++++++++++++++-
 2 files changed, 70 insertions(+), 1 deletion(-)
 create mode 100644 python/gtsam_tests/testScenarioRunner.py

diff --git a/python/gtsam_tests/testScenarioRunner.py b/python/gtsam_tests/testScenarioRunner.py
new file mode 100644
index 000000000..2e1b47202
--- /dev/null
+++ b/python/gtsam_tests/testScenarioRunner.py
@@ -0,0 +1,30 @@
+import math
+import unittest
+import numpy as np
+
+import gtsam
+
+class TestScenarioRunner(unittest.TestCase):
+    def setUp(self):
+        self.g = 10  # simple gravity constant
+
+    def test_loop_runner(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)
+
+        dt = 0.1
+        params = gtsam.PreintegrationParams.MakeSharedU(self.g)
+        runner = gtsam.ScenarioRunner(gtsam.ScenarioPointer(scenario), params, dt)
+
+        # Test specific force at time 0: a is pointing up 
+        t = 0.0
+        a = w * v
+        np.testing.assert_almost_equal(np.array([0, 0, a + self.g]), runner.actualSpecificForce(t))
+
+if __name__ == '__main__':
+    unittest.main()
diff --git a/python/handwritten/navigation/Scenario.cpp b/python/handwritten/navigation/Scenario.cpp
index f720bfd2e..57a383283 100644
--- a/python/handwritten/navigation/Scenario.cpp
+++ b/python/handwritten/navigation/Scenario.cpp
@@ -19,12 +19,20 @@
 #define NO_IMPORT_ARRAY
 #include <numpy_eigen/NumpyEigenConverter.hpp>
 
-#include "gtsam/navigation/Scenario.h"
+#include "gtsam/navigation/ScenarioRunner.h"
 
 using namespace boost::python;
 using namespace gtsam;
 
+// Create const Scenario pointer from ConstantTwistScenario
+static const Scenario* ScenarioPointer(const ConstantTwistScenario& scenario) {
+  return static_cast<const Scenario*>(&scenario);
+}
+
 void exportScenario() {
+  // NOTE(frank): Abstract classes need boost::noncopyable
+  class_<Scenario, boost::noncopyable>("Scenario", no_init);
+
   // TODO(frank): figure out how to do inheritance
   class_<ConstantTwistScenario>("ConstantTwistScenario",
                                 init<const Vector3&, const Vector3&>())
@@ -35,4 +43,35 @@ void exportScenario() {
       .def("rotation", &Scenario::rotation)
       .def("velocity_b", &Scenario::velocity_b)
       .def("acceleration_b", &Scenario::acceleration_b);
+
+  // NOTE(frank): https://wiki.python.org/moin/boost.python/CallPolicy
+  def("ScenarioPointer", &ScenarioPointer,
+      return_value_policy<reference_existing_object>());
+
+  class_<PreintegrationParams, boost::shared_ptr<PreintegrationParams> >(
+      "PreintegrationParams", init<const Vector3&>())
+      .def_readwrite("gyroscopeCovariance",
+                     &PreintegrationParams::gyroscopeCovariance)
+      .def_readwrite("omegaCoriolis", &PreintegrationParams::omegaCoriolis)
+      .def_readwrite("body_P_sensor", &PreintegrationParams::body_P_sensor)
+      .def_readwrite("accelerometerCovariance",
+                     &PreintegrationParams::accelerometerCovariance)
+      .def_readwrite("integrationCovariance",
+                     &PreintegrationParams::integrationCovariance)
+      .def_readwrite("use2ndOrderCoriolis",
+                     &PreintegrationParams::use2ndOrderCoriolis)
+      .def_readwrite("n_gravity", &PreintegrationParams::n_gravity)
+
+      .def("MakeSharedD", &PreintegrationParams::MakeSharedD)
+      .staticmethod("MakeSharedD")
+      .def("MakeSharedU", &PreintegrationParams::MakeSharedU)
+      .staticmethod("MakeSharedU");
+
+  class_<ScenarioRunner>(
+      "ScenarioRunner",
+      init<const Scenario*, const boost::shared_ptr<PreintegrationParams>&,
+           double>())
+      .def("actualSpecificForce", &ScenarioRunner::actualSpecificForce)
+      .def("measuredAngularVelocity", &ScenarioRunner::measuredAngularVelocity)
+      .def("measuredSpecificForce", &ScenarioRunner::measuredSpecificForce);
 }