diff --git a/open3d_conversions/.gitignore b/open3d_conversions/.gitignore new file mode 100644 index 0000000..45fce1d --- /dev/null +++ b/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 diff --git a/open3d_conversions/CMakeLists.txt b/open3d_conversions/CMakeLists.txt index 057d169..b1f4c53 100644 --- a/open3d_conversions/CMakeLists.txt +++ b/open3d_conversions/CMakeLists.txt @@ -1,6 +1,12 @@ 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 @@ -8,12 +14,14 @@ find_package(catkin REQUIRED COMPONENTS 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( @@ -21,6 +29,7 @@ include_directories( ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${Open3D_INCLUDE_DIRS} + ${pybind11_INCLUDE_DIR} ) # C++ library @@ -28,6 +37,25 @@ 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() + + diff --git a/open3d_conversions/src/python/open3d_conversions_pybind.cpp b/open3d_conversions/src/python/open3d_conversions_pybind.cpp new file mode 100644 index 0000000..4e9eda2 --- /dev/null +++ b/open3d_conversions/src/python/open3d_conversions_pybind.cpp @@ -0,0 +1,60 @@ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "open3d_conversions/open3d_conversions.h" +#include "point_cloud2_iterator.h" + +PYBIND11_MAKE_OPAQUE(std::vector); + +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")); +} \ No newline at end of file diff --git a/open3d_conversions/src/python/point_cloud2_iterator.h b/open3d_conversions/src/python/point_cloud2_iterator.h new file mode 100644 index 0000000..470b42e --- /dev/null +++ b/open3d_conversions/src/python/point_cloud2_iterator.h @@ -0,0 +1,44 @@ +#include +#include +#include +#include + +namespace py = pybind11; + +void bind_point_cloud2_modifier(py::module &m) { + py::class_(m, "PointCloud2Modifier") + .def(py::init()) + .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 field_args; + for (const auto &arg : args) { + field_args.push_back(arg.cast()); + } + self.setPointCloud2Fields(n_fields, field_args.data()); + }) + .def("setPointCloud2FieldsByString", + [](sensor_msgs::PointCloud2Modifier &self, int n_fields, py::args args) { + std::vector field_args; + for (const auto &arg : args) { + field_args.push_back(arg.cast()); + } + self.setPointCloud2FieldsByString(n_fields, field_args.data()); + }); +} + +template +void bind_point_cloud2_iterator(py::module &m, const std::string &name) { + using PointCloud2IteratorType = sensor_msgs::PointCloud2Iterator; + py::class_(m, name.c_str()) + .def(py::init()) + .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]; }); +} diff --git a/open3d_conversions/src/python/stl_vector_eigen.h b/open3d_conversions/src/python/stl_vector_eigen.h new file mode 100644 index 0000000..58cf325 --- /dev/null +++ b/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 +#include + +// pollute namespace with py +#include +namespace py = pybind11; +#include +#include +#include +#include +#include + +namespace pybind11 { + +template , typename... Args> +py::class_ 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_; + Class_ cl(m, name.c_str(), std::forward(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 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 buffer. +// - Directly using templates for the py::array_t and py::array_t +// and etc. doesn't work. The current solution is to explicitly implement +// bindings for each py array types. +template +std::vector py_array_to_vectors_double( + py::array_t array) { + int64_t eigen_vector_size = EigenVector::SizeAtCompileTime; + if (array.ndim() != 2 || array.shape(1) != eigen_vector_size) { + throw py::cast_error(); + } + std::vector 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(&array_unchecked(i, 0)); + } + return eigen_vectors; +} + +} // namespace pybind11 + +template , + typename holder_type = std::unique_ptr, + typename InitFunc> +py::class_ 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>( + m, bind_name, py::buffer_protocol(), py::module_local()); + vec.def(py::init(init_func)); + vec.def_buffer([](std::vector &v) -> py::buffer_info { + size_t rows = EigenVector::RowsAtCompileTime; + return py::buffer_info(v.data(), sizeof(Scalar), py::format_descriptor::format(), 2, + {v.size(), rows}, {sizeof(EigenVector), sizeof(Scalar)}); + }); + vec.def("__repr__", [repr_name](const std::vector &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 &v) { return std::vector(v); }); + vec.def("__deepcopy__", + [](std::vector &v) { return std::vector(v); }); + + // py::detail must be after custom constructor + using Class_ = py::class_>; + py::detail::vector_if_copy_constructible(vec); + py::detail::vector_if_equal_operator(vec); + py::detail::vector_modifiers(vec); + py::detail::vector_accessor(vec); + + return vec; +} \ No newline at end of file