Add PyBind setters for Cpp implementations - motion_state

p3iv_types/package.xml

@@ -13,7 +13,7 @@
   <build_depend>mrt_cmake_modules</build_depend>
   <test_depend>gtest</test_depend>
 
-  <depend>pybind</depend>
+  <depend>pybind11-dev</depend>
   <depend>util_probability</depend>
 
   <export>

p3iv_types/python_api/py_motion_state.cpp

@@ -0,0 +1,40 @@
+#include "py_motion_state.hpp"
+#include <iostream>
+#include <memory>
+#include <glog/logging.h>
+#include <pybind11/pybind11.h>
+
+using namespace p3iv_types;
+namespace py = pybind11;
+
+
+// The module name here *must* match the name of the python project. You can use the PYTHON_API_MODULE_NAME definition.
+PYBIND11_MODULE(PYTHON_API_MODULE_NAME, m) {
+    m.doc() = R"pbdoc(
+            Python bindings for C++ Implementation
+            -----------------------
+            .. currentmodule:: PYTHON_API_MODULE_NAME
+            .. autosummary::
+               :toctree: _generate
+
+        )pbdoc";
+
+    py::class_<PyMotionState>(m, "PyMotionState", py::module_local())
+        .def(py::init<>())
+        .def("setPosition", &PyMotionState::setPosition, py::arg("mean"), py::arg("covariance"))
+        .def("setYaw", &PyMotionState::setYaw, py::arg("mean"), py::arg("covariance"))
+        .def("setVelocity", &PyMotionState::setVelocity, py::arg("mean"), py::arg("covariance"));
+
+    py::class_<PyMotionStateArray>(m, "PyMotionStateArray", py::module_local())
+        .def(py::init<>())
+        .def("setPosition", &PyMotionStateArray::setPosition, py::arg("mean"), py::arg("covariance"))
+        .def("setYaw", &PyMotionStateArray::setYaw, py::arg("mean"), py::arg("covariance"))
+        .def("setVelocity", &PyMotionStateArray::setVelocity, py::arg("mean"), py::arg("covariance"));
+
+
+#ifdef VERSION_INFO
+    m.attr("__version__") = VERSION_INFO;
+#else
+    m.attr("__version__") = "dev";
+#endif
+}

p3iv_types/python_api/py_motion_state.hpp

@@ -0,0 +1,62 @@
+#pragma once
+#include <pybind11/pybind11.h>
+#include "motion_state.hpp"
+#include "pybind_utils.hpp"
+
+namespace py = pybind11;
+using PyArray = py::array_t<double>;
+
+namespace p3iv_types {
+
+using namespace pybind_utils;
+
+struct PyMotionState : MotionState {
+
+    void setPosition(PyArray mean, PyArray covariance) {
+        std::vector<double> m = pyArray2Vector<double>(mean);
+        std::vector<double> c = pyArray2Vector<double>(covariance);
+        this->position.setMean(m[0], m[1]);
+        this->position.setCovariance(c[0], c[1], c[2], c[3]);
+    }
+
+    void setYaw(PyArray mean, PyArray covariance) {
+        std::vector<double> m = pyArray2Vector<double>(mean);
+        std::vector<double> c = pyArray2Vector<double>(covariance);
+        this->yaw.setMean(m[0]);
+        this->yaw.setCovariance(c[0]);
+    }
+
+    void setVelocity(PyArray mean, PyArray covariance) {
+        std::vector<double> m = pyArray2Vector<double>(mean);
+        std::vector<double> c = pyArray2Vector<double>(covariance);
+        this->velocity.setMean(m[0], m[1]);
+        this->velocity.setCovariance(c[0], c[1], c[2], c[3]);
+    }
+};
+
+
+struct PyMotionStateArray : MotionStateArray {
+
+    void setPosition(PyArray mean, PyArray covariance) {
+        std::vector<double> m = pyArray2Vector<double>(mean);
+        std::vector<double> c = pyArray2Vector<double>(covariance);
+        this->position.setMean(m);
+        this->position.setCovariance(c);
+    }
+
+    void setYaw(PyArray mean, PyArray covariance) {
+        std::vector<double> m = pyArray2Vector<double>(mean);
+        std::vector<double> c = pyArray2Vector<double>(covariance);
+        this->yaw.setMean(m);
+        this->yaw.setCovariance(c);
+    }
+
+    void setVelocity(PyArray mean, PyArray covariance) {
+        std::vector<double> m = pyArray2Vector<double>(mean);
+        std::vector<double> c = pyArray2Vector<double>(covariance);
+        this->velocity.setMean(m);
+        this->velocity.setCovariance(c);
+    }
+};
+
+} // namespace p3iv_types

p3iv_types/include/p3iv_types/python_converters/pybind_utils.hpp → p3iv_types/python_api/pybind_utils.hpp

@@ -9,10 +9,10 @@
 namespace py = pybind11;
 
 namespace p3iv_types {
-namespace python_converters {
+namespace pybind_utils {
 
 template <typename T>
-std::vector<T> numpyArray2Vector(const py::array_t<T>& arr) {
+std::vector<T> pyArray2Vector(const py::array_t<T>& arr) {
     // check dimensions first
     if (arr.ndim() != 1) {
         throw std::invalid_argument(
@@ -30,12 +30,13 @@ std::vector<T> numpyArray2Vector(const py::array_t<T>& arr) {
 }
 
 template <typename T>
-py::array_t<T> vector2NumpyArray(const std::vector<T>& vec) {
+py::array_t<T> vector2pyArray(const std::vector<T>& vec) {
     py::array_t<T> output(vec.size(), vec.data());
     return output;
 }
 
-internal::VectorPoint2d<double> numpyArray2VectorPoint2d(const py::array_t<double>& arr) {
+
+internal::VectorPoint2d<double> pyArray2VectorPoint2d(const py::array_t<double>& arr) {
     // check dimensions first
     if (arr.ndim() != 2) {
         throw std::invalid_argument(
@@ -54,18 +55,14 @@ internal::VectorPoint2d<double> numpyArray2VectorPoint2d(const py::array_t<doubl
     std::vector<internal::Point2d<double>> vec;
     vec.reserve(numberOfPoints);
     for (size_t i = 0; i < numberOfPoints; i++) {
-        vec.emplace_back(numpyArray2Point2d(rawArray));
+        internal::Point2d<double> p = {rawArray(i, 0), rawArray(i, 1)};
+        vec.emplace_back(p);
     }
 
     internal::VectorPoint2d<double> v(vec);
     return v;
 }
 
-inline internal::Point2d<double> numpyArray2Point2d(const py::array_t<double>& pointarray) {
-    internal::Point2d<double> p;
-    p(0) = pointarray.at(0);
-    p(1) = pointarray.at(1);
-    return p;
-}
-} // namespace python_converters
+
+} // namespace pybind_utils
 } // namespace p3iv_types

p3iv_types/test/test_py_motion_state.py

@@ -0,0 +1,28 @@
+import unittest
+import numpy as np
+from p3iv_types.py_motion_state import PyMotionState, PyMotionStateArray
+
+
+class PyMotionStateTest(unittest.TestCase):
+    def test_empty(self):
+        m = PyMotionState()
+        mean = np.asarray([4.0, 0.0])
+        cov = np.asarray([16.0, 0.0, 0.0, 16.0])
+        m.setPosition(mean, cov)
+        m.setYaw(mean[:1], cov[:1])
+        m.setVelocity(mean, cov)
+
+
+class PyMotionStateArrayTest(unittest.TestCase):
+    def test_empty(self):
+        PyMotionStateArray()
+
+    def test_mp(self):
+        m = PyMotionStateArray()
+        mean = np.asarray([4.0, 0.0, 4.0, 0.0])
+        cov = np.asarray([16.0, 0.0, 0.0, 16.0, 16.0, 0.0, 0.0, 16.0])
+        m.setPosition(mean, cov)
+
+
+if __name__ == "__main__":
+    unittest.main()