Wrapped ConstantTwistScenario

python/gtsam_tests/testScenario.py

@@ -0,0 +1,32 @@
+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()))
+
+if __name__ == '__main__':
+    unittest.main()

python/handwritten/exportgtsam.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -10,8 +10,8 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @brief exports the python module 
- * @author Andrew Melim 
+ * @brief exports the python module
+ * @author Andrew Melim
  * @author Ellon Paiva Mendes (LAAS-CNRS)
  **/
 
@@ -20,10 +20,10 @@
 
 #include <numpy_eigen/NumpyEigenConverter.hpp>
 
-// Base
+// base
 void exportFastVectors();
 
-// Geometry
+// geometry
 void exportPoint2();
 void exportPoint3();
 void exportRot2();
@@ -34,24 +34,28 @@ void exportPinholeBaseK();
 void exportPinholeCamera();
 void exportCal3_S2();
 
-// Inference
+// inference
 void exportSymbol();
 
-// Linear
+// linear
 void exportNoiseModels();
 
-// Nonlinear
+// nonlinear
 void exportValues();
 void exportNonlinearFactor();
 void exportNonlinearFactorGraph();
 void exportLevenbergMarquardtOptimizer();
 void exportISAM2();
 
-// Slam
+// slam
 void exportPriorFactors();
 void exportBetweenFactors();
 void exportGenericProjectionFactor();
 
+// navigation
+void exportScenario();
+
+
 // Utils (or Python wrapper specific functions)
 void registerNumpyEigenConversions();
 
@@ -59,14 +63,14 @@ void registerNumpyEigenConversions();
 
 BOOST_PYTHON_MODULE(_libgtsam_python){
 
-  // NOTE: We need to call import_array1() instead of import_array() to support both python 2 
-  //       and 3. The reason is that BOOST_PYTHON_MODULE puts all its contents in a function 
-  //       returning void, and import_array() is a macro that when expanded for python 3, adds 
-  //       a 'return __null' statement to that function. For more info check files: 
+  // NOTE: We need to call import_array1() instead of import_array() to support both python 2
+  //       and 3. The reason is that BOOST_PYTHON_MODULE puts all its contents in a function
+  //       returning void, and import_array() is a macro that when expanded for python 3, adds
+  //       a 'return __null' statement to that function. For more info check files:
   //       boost/python/module_init.hpp and numpy/__multiarray_api.h (bottom of the file).
   // Should be the first thing to be done
   import_array1();
-  
+
   registerNumpyEigenConversions();
 
   exportFastVectors();
@@ -94,4 +98,6 @@ BOOST_PYTHON_MODULE(_libgtsam_python){
   exportPriorFactors();
   exportBetweenFactors();
   exportGenericProjectionFactor();
-}
+
+  exportScenario();
+}

python/handwritten/navigation/Scenario.cpp

@@ -0,0 +1,38 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @brief wraps ConstantTwistScenario class to python
+ * @author Frank Dellaert
+ **/
+
+#include <boost/python.hpp>
+
+#define NO_IMPORT_ARRAY
+#include <numpy_eigen/NumpyEigenConverter.hpp>
+
+#include "gtsam/navigation/Scenario.h"
+
+using namespace boost::python;
+using namespace gtsam;
+
+void exportScenario() {
+  // TODO(frank): figure out how to do inheritance
+  class_<ConstantTwistScenario>("ConstantTwistScenario",
+                                init<const Vector3&, const Vector3&>())
+      .def("pose", &Scenario::pose)
+      .def("omega_b", &Scenario::omega_b)
+      .def("velocity_n", &Scenario::velocity_n)
+      .def("acceleration_n", &Scenario::acceleration_n)
+      .def("rotation", &Scenario::rotation)
+      .def("velocity_b", &Scenario::velocity_b)
+      .def("acceleration_b", &Scenario::acceleration_b);
+}