WIP! Python bindings

open3d_conversions/.gitignore

@@ -0,0 +1,12 @@
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+!.vscode/*.code-snippets
+
+# Local History for Visual Studio Code
+.history/
+
+# Built Visual Studio Code Extensions
+*.vsix

open3d_conversions/CMakeLists.txt

@@ -1,33 +1,61 @@
 cmake_minimum_required(VERSION 3.5.0)
 project(open3d_conversions)
 
+set(CMAKE_BUILD_TYPE Release)
+option(BUILD_PYTHON_BINDINGS "Build python version" ON)
+
+set(CMAKE_CXX_STANDARD 14)
+set(PYBIND11_CPP_STANDARD -std=c++14)
+
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   sensor_msgs
 )
 
 find_package(Eigen3 REQUIRED)
 find_package(Open3D REQUIRED)
+find_package(pybind11 REQUIRED)
+
 
 catkin_package(
  INCLUDE_DIRS include
  LIBRARIES open3d_conversions
  CATKIN_DEPENDS roscpp sensor_msgs
- DEPENDS EIGEN3 Open3D
+ DEPENDS EIGEN3 Open3D pybind11 
 )
 
 include_directories(
   include
   ${catkin_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIR}
   ${Open3D_INCLUDE_DIRS}
+  ${pybind11_INCLUDE_DIR}
 )
 
 # C++ library
 add_library(open3d_conversions src/open3d_conversions.cpp)
 add_dependencies(open3d_conversions ${open3d_conversions_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(open3d_conversions ${catkin_LIBRARIES} ${Open3D_LIBRARIES})
 
+
+## Adapted from fastgicp
+### Python bindings ###
+if(BUILD_PYTHON_BINDINGS)
+  pybind11_add_module(open3d_conversions_py
+    src/python/open3d_conversions_pybind.cpp
+  )
+  target_include_directories(open3d_conversions_py PUBLIC
+    include
+    ${EIGEN3_INCLUDE_DIR}
+    ${Open3D_INCLUDE_DIRS}
+    ${pybind11_INCLUDE_DIR}
+  )
+  target_link_libraries(open3d_conversions_py PRIVATE
+    open3d_conversions
+  )
+endif()
+
+
 # Install
 install(TARGETS open3d_conversions
         ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
@@ -37,8 +65,11 @@ install(TARGETS open3d_conversions
 install(DIRECTORY include/open3d_conversions/
         DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
 
+
 # Tests
 if (CATKIN_ENABLE_TESTING)
   catkin_add_gtest(test_open3d_conversions test/test_open3d_conversions.cpp)
   target_link_libraries(test_open3d_conversions open3d_conversions ${catkin_LIBRARIES} ${Open3D_LIBRARIES})
 endif()
+
+

open3d_conversions/src/python/open3d_conversions_pybind.cpp

@@ -0,0 +1,60 @@
+
+#include <pybind11/pybind11.h> 
+#include <pybind11/detail/internals.h>
+#include <pybind11/eigen.h>
+#include <pybind11/functional.h>
+#include <pybind11/chrono.h>
+#include <pybind11/numpy.h>
+#include <pybind11/operators.h>
+#include <pybind11/stl.h>
+#include <Eigen/Core>
+#include "open3d_conversions/open3d_conversions.h"
+#include "point_cloud2_iterator.h"
+
+PYBIND11_MAKE_OPAQUE(std::vector<Eigen::Vector3d>);
+
+PYBIND11_MODULE(open3d_conversions_py, m) {
+    namespace py = pybind11;
+
+    m.def("open3dToRos",
+          [](const open3d::geometry::PointCloud& pointcloud, sensor_msgs::PointCloud2& ros_pc2,
+             std::string frame_id) {
+              open3d_conversions::open3dToRos(pointcloud, ros_pc2, frame_id);
+          },
+          py::arg("pointcloud"), py::arg("ros_pc2"), py::arg("frame_id") = "open3d_pointcloud");
+
+    m.def("rosToOpen3d",
+          [](const sensor_msgs::PointCloud2ConstPtr& ros_pc2, open3d::geometry::PointCloud& o3d_pc,
+             bool skip_colors) {
+              open3d_conversions::rosToOpen3d(ros_pc2, o3d_pc, skip_colors);
+          },
+          py::arg("ros_pc2"), py::arg("o3d_pc"), py::arg("skip_colors") = false);
+
+    m.def("open3dToRos_t",
+          [](const open3d::t::geometry::PointCloud& pointcloud, sensor_msgs::PointCloud2& ros_pc2,
+             std::string frame_id, int t_num_fields) {
+              open3d_conversions::open3dToRos(pointcloud, ros_pc2, frame_id, t_num_fields);
+          },
+          py::arg("pointcloud"), py::arg("ros_pc2"), py::arg("frame_id") = "open3d_pointcloud",
+          py::arg("t_num_fields") = 2);
+
+    m.def("rosToOpen3d_t",
+          [](const sensor_msgs::PointCloud2ConstPtr& ros_pc2, open3d::t::geometry::PointCloud& o3d_pc,
+             bool skip_colors) {
+              open3d_conversions::rosToOpen3d(ros_pc2, o3d_pc, skip_colors);
+          },
+          py::arg("ros_pc2"), py::arg("o3d_pc"), py::arg("skip_colors") = false);
+
+    m.def("addPointField",
+          [](sensor_msgs::PointCloud2& cloud_msg, const std::string& name,
+             int count, int datatype, int offset) {
+              addPointField(cloud_msg, name, count, datatype, offset);
+          },
+          py::arg("cloud_msg"), py::arg("name"), py::arg("count"),py::arg("datatype"), py::arg("offset"));
+
+     m.def("sizeOfPointField",
+          [](int datatype) {
+              sizeOfPointField(datatype);
+          },
+          py::arg("datatype"));
+}

open3d_conversions/src/python/point_cloud2_iterator.h

@@ -0,0 +1,44 @@
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/point_cloud2_iterator.h>
+
+namespace py = pybind11;
+
+void bind_point_cloud2_modifier(py::module &m) {
+    py::class_<sensor_msgs::PointCloud2Modifier>(m, "PointCloud2Modifier")
+        .def(py::init<sensor_msgs::PointCloud2 &>())
+        .def("size", &sensor_msgs::PointCloud2Modifier::size)
+        .def("reserve", &sensor_msgs::PointCloud2Modifier::reserve)
+        .def("resize", &sensor_msgs::PointCloud2Modifier::resize)
+        .def("clear", &sensor_msgs::PointCloud2Modifier::clear)
+        .def("setPointCloud2Fields",
+            [](sensor_msgs::PointCloud2Modifier &self, int n_fields, py::args args) {
+                std::vector<std::string> field_args;
+                for (const auto &arg : args) {
+                    field_args.push_back(arg.cast<std::string>());
+                }
+                self.setPointCloud2Fields(n_fields, field_args.data());
+            })
+        .def("setPointCloud2FieldsByString",
+            [](sensor_msgs::PointCloud2Modifier &self, int n_fields, py::args args) {
+                std::vector<std::string> field_args;
+                for (const auto &arg : args) {
+                    field_args.push_back(arg.cast<std::string>());
+                }
+                self.setPointCloud2FieldsByString(n_fields, field_args.data());
+            });
+}
+
+template <typename T>
+void bind_point_cloud2_iterator(py::module &m, const std::string &name) {
+    using PointCloud2IteratorType = sensor_msgs::PointCloud2Iterator<T>;
+    py::class_<PointCloud2IteratorType>(m, name.c_str())
+        .def(py::init<sensor_msgs::PointCloud2 &, const std::string &>())
+        .def("__iter__", [](const PointCloud2IteratorType &iter) -> const PointCloud2IteratorType & { return iter; })
+        .def("__next__", [](PointCloud2IteratorType &iter) -> T & { return *++iter; })
+        .def("size", &PointCloud2IteratorType::size)
+        .def("reserve", &PointCloud2IteratorType::reserve)
+        .def("resize", &PointCloud2IteratorType::resize)
+        .def("__getitem__", [](const PointCloud2IteratorType &iter, size_t i) -> T & { return iter[i]; });
+}

open3d_conversions/src/python/stl_vector_eigen.h

@@ -0,0 +1,117 @@
+// ----------------------------------------------------------------------------
+// NOTE: This fily has been adapted from the Open3D project, but copyright
+// still belongs to Open3D. All rights reserved
+// ----------------------------------------------------------------------------
+// -                        Open3D: www.open3d.org                            -
+// ----------------------------------------------------------------------------
+// The MIT License (MIT)
+//
+// Copyright (c) 2018-2021 www.open3d.org
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+// IN THE SOFTWARE.
+// ----------------------------------------------------------------------------
+#pragma once
+#include <Eigen/Core>
+#include <vector>
+
+// pollute namespace with py
+#include <pybind11/pybind11.h>
+namespace py = pybind11;
+#include <pybind11/eigen.h>
+#include <pybind11/numpy.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+#include <pybind11/stl_bind.h>
+
+namespace pybind11 {
+
+template <typename Vector, typename holder_type = std::unique_ptr<Vector>, typename... Args>
+py::class_<Vector, holder_type> bind_vector_without_repr(py::module &m,
+                                                         std::string const &name,
+                                                         Args &&...args) {
+    // hack function to disable __repr__ for the convenient function
+    // bind_vector()
+    using Class_ = py::class_<Vector, holder_type>;
+    Class_ cl(m, name.c_str(), std::forward<Args>(args)...);
+    cl.def(py::init<>());
+    cl.def(
+        "__bool__", [](const Vector &v) -> bool { return !v.empty(); },
+        "Check whether the list is nonempty");
+    cl.def("__len__", &Vector::size);
+    return cl;
+}
+
+// - This function is used by Pybind for std::vector<SomeEigenType> constructor.
+//   This optional constructor is added to avoid too many Python <-> C++ API
+//   calls when the vector size is large using the default biding method.
+//   Pybind matches np.float64 array to py::array_t<double> buffer.
+// - Directly using templates for the py::array_t<double> and py::array_t<int>
+//   and etc. doesn't work. The current solution is to explicitly implement
+//   bindings for each py array types.
+template <typename EigenVector>
+std::vector<EigenVector> py_array_to_vectors_double(
+    py::array_t<double, py::array::c_style | py::array::forcecast> array) {
+    int64_t eigen_vector_size = EigenVector::SizeAtCompileTime;
+    if (array.ndim() != 2 || array.shape(1) != eigen_vector_size) {
+        throw py::cast_error();
+    }
+    std::vector<EigenVector> eigen_vectors(array.shape(0));
+    auto array_unchecked = array.mutable_unchecked<2>();
+    for (auto i = 0; i < array_unchecked.shape(0); ++i) {
+        eigen_vectors[i] = Eigen::Map<EigenVector>(&array_unchecked(i, 0));
+    }
+    return eigen_vectors;
+}
+
+}  // namespace pybind11
+
+template <typename EigenVector,
+          typename Vector = std::vector<EigenVector>,
+          typename holder_type = std::unique_ptr<Vector>,
+          typename InitFunc>
+py::class_<Vector, holder_type> pybind_eigen_vector_of_vector(py::module &m,
+                                                              const std::string &bind_name,
+                                                              const std::string &repr_name,
+                                                              InitFunc init_func) {
+    using Scalar = typename EigenVector::Scalar;
+    auto vec = py::bind_vector_without_repr<std::vector<EigenVector>>(
+        m, bind_name, py::buffer_protocol(), py::module_local());
+    vec.def(py::init(init_func));
+    vec.def_buffer([](std::vector<EigenVector> &v) -> py::buffer_info {
+        size_t rows = EigenVector::RowsAtCompileTime;
+        return py::buffer_info(v.data(), sizeof(Scalar), py::format_descriptor<Scalar>::format(), 2,
+                               {v.size(), rows}, {sizeof(EigenVector), sizeof(Scalar)});
+    });
+    vec.def("__repr__", [repr_name](const std::vector<EigenVector> &v) {
+        return repr_name + std::string(" with ") + std::to_string(v.size()) +
+               std::string(" elements.\n") + std::string("Use numpy.asarray() to access data.");
+    });
+    vec.def("__copy__", [](std::vector<EigenVector> &v) { return std::vector<EigenVector>(v); });
+    vec.def("__deepcopy__",
+            [](std::vector<EigenVector> &v) { return std::vector<EigenVector>(v); });
+
+    // py::detail must be after custom constructor
+    using Class_ = py::class_<Vector, std::unique_ptr<Vector>>;
+    py::detail::vector_if_copy_constructible<Vector, Class_>(vec);
+    py::detail::vector_if_equal_operator<Vector, Class_>(vec);
+    py::detail::vector_modifiers<Vector, Class_>(vec);
+    py::detail::vector_accessor<Vector, Class_>(vec);
+
+    return vec;
+}