Successful wrap of Scenario

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()

gtsam.h

@@ -2666,9 +2666,11 @@ virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{
 };
 
 virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor {
-  Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model,
+  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(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;
 };
 
 //*************************************************************************

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));
 }