diff --git a/src/python/gui.cpp b/src/python/gui.cpp index 35ceb6f1..e2dd78a9 100644 --- a/src/python/gui.cpp +++ b/src/python/gui.cpp @@ -10,6 +10,23 @@ #include #endif +void pointcloud_wrapper(const DepthImage& depth_image, const Eigen::Matrix3d& intrinsic_matrix, const Eigen::Matrix4d& tf, double far_plane, py::array_t& in_results) { + if (in_results.ndim() != 2) + throw std::runtime_error("Results should be a 2-D Numpy array!"); + + auto buf = in_results.request(); + double* ptr = (double*)buf.ptr; + + size_t N = in_results.shape()[0]; + size_t M = in_results.shape()[1]; + + std::vector v_p = robot_dart::gui::point_cloud_from_depth_array_vector(depth_image, intrinsic_matrix, tf, far_plane); + if (v_p.size() != (N * M)) + throw std::runtime_error("Wrong size of Numpy array!"); + + memcpy(ptr, v_p.data(), v_p.size() * sizeof(double)); +} + namespace robot_dart { namespace python { #ifdef GRAPHIC @@ -293,11 +310,12 @@ namespace robot_dart { sm.def("save_png_image", static_cast(&gui::save_png_image)); sm.def("save_png_image", static_cast(&gui::save_png_image)); sm.def("convert_rgb_to_grayscale", gui::convert_rgb_to_grayscale); - sm.def("point_cloud_from_depth_array", gui::point_cloud_from_depth_array_vector, + sm.def("point_cloud_from_depth_array", pointcloud_wrapper, py::arg("depth_image"), py::arg("intrinsic_matrix"), py::arg("tf"), - py::arg("far_plane") = 1000.); + py::arg("far_plane") = 1000., + py::arg("array")); // Material class using Material = gui::magnum::gs::Material;