From c25e1e6b73339fa9f7811eb17f37717fab94b31a Mon Sep 17 00:00:00 2001 From: Frank Date: Tue, 26 Jan 2016 14:15:39 -0800 Subject: [PATCH] Wrapped ConstantTwistScenario --- python/gtsam_tests/testScenario.py | 32 ++++++++++++++++++ python/handwritten/exportgtsam.cpp | 36 +++++++++++--------- python/handwritten/navigation/Scenario.cpp | 38 ++++++++++++++++++++++ 3 files changed, 91 insertions(+), 15 deletions(-) create mode 100644 python/gtsam_tests/testScenario.py create mode 100644 python/handwritten/navigation/Scenario.cpp diff --git a/python/gtsam_tests/testScenario.py b/python/gtsam_tests/testScenario.py new file mode 100644 index 000000000..e78121241 --- /dev/null +++ b/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() diff --git a/python/handwritten/exportgtsam.cpp b/python/handwritten/exportgtsam.cpp index da8382d71..3f74cc56a 100644 --- a/python/handwritten/exportgtsam.cpp +++ b/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 -// 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(); -} \ No newline at end of file + + exportScenario(); +} diff --git a/python/handwritten/navigation/Scenario.cpp b/python/handwritten/navigation/Scenario.cpp new file mode 100644 index 000000000..f720bfd2e --- /dev/null +++ b/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 + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/navigation/Scenario.h" + +using namespace boost::python; +using namespace gtsam; + +void exportScenario() { + // TODO(frank): figure out how to do inheritance + class_("ConstantTwistScenario", + init()) + .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); +}