From 838bbc1140054e5d39a78a6301120050ee6e8d26 Mon Sep 17 00:00:00 2001 From: Arne Peters Date: Wed, 2 Mar 2022 16:21:03 +0100 Subject: [PATCH 1/2] fixed wrong point clound size info --- src/k4a_ros_device.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/k4a_ros_device.cpp b/src/k4a_ros_device.cpp index 3229a698..54d1fb2b 100644 --- a/src/k4a_ros_device.cpp +++ b/src/k4a_ros_device.cpp @@ -666,8 +666,6 @@ k4a_result_t K4AROSDevice::getPointCloud(const k4a::capture& capture, sensor_msg k4a_result_t K4AROSDevice::fillColorPointCloud(const k4a::image& pointcloud_image, const k4a::image& color_image, sensor_msgs::PointCloud2Ptr& point_cloud) { - point_cloud->height = pointcloud_image.get_height_pixels(); - point_cloud->width = pointcloud_image.get_width_pixels(); point_cloud->is_dense = false; point_cloud->is_bigendian = false; @@ -692,6 +690,11 @@ k4a_result_t K4AROSDevice::fillColorPointCloud(const k4a::image& pointcloud_imag pcd_modifier.resize(point_count); + // Restore actual dimensions as pcd_modifier.resize(n) sets the cloud size to n x 1 + point_cloud->height = pointcloud_image.get_height_pixels(); + point_cloud->width = pointcloud_image.get_width_pixels(); + point_cloud->row_step = pointcloud_image.get_width_pixels() * point_cloud->point_step; + const int16_t* point_cloud_buffer = reinterpret_cast(pointcloud_image.get_buffer()); const uint8_t* color_buffer = color_image.get_buffer(); @@ -724,8 +727,6 @@ k4a_result_t K4AROSDevice::fillColorPointCloud(const k4a::image& pointcloud_imag k4a_result_t K4AROSDevice::fillPointCloud(const k4a::image& pointcloud_image, sensor_msgs::PointCloud2Ptr& point_cloud) { - point_cloud->height = pointcloud_image.get_height_pixels(); - point_cloud->width = pointcloud_image.get_width_pixels(); point_cloud->is_dense = false; point_cloud->is_bigendian = false; @@ -740,6 +741,11 @@ k4a_result_t K4AROSDevice::fillPointCloud(const k4a::image& pointcloud_image, se pcd_modifier.resize(point_count); + // Set actual dimensions as pcd_modifier.resize(n) sets the cloud size to n x 1 + point_cloud->height = pointcloud_image.get_height_pixels(); + point_cloud->width = pointcloud_image.get_width_pixels(); + point_cloud->row_step = pointcloud_image.get_width_pixels() * point_cloud->point_step; + const int16_t* point_cloud_buffer = reinterpret_cast(pointcloud_image.get_buffer()); for (size_t i = 0; i < point_count; i++, ++iter_x, ++iter_y, ++iter_z) From e05426e2ffb06cea3d41131f188fdc9243c66481 Mon Sep 17 00:00:00 2001 From: Arne Peters Date: Mon, 24 Oct 2022 18:44:38 +0200 Subject: [PATCH 2/2] resizing point cloud data after initializing iterators was a bad idea --- src/k4a_ros_device.cpp | 24 ++++++++++-------------- 1 file changed, 10 insertions(+), 14 deletions(-) diff --git a/src/k4a_ros_device.cpp b/src/k4a_ros_device.cpp index 54d1fb2b..8e3f4594 100644 --- a/src/k4a_ros_device.cpp +++ b/src/k4a_ros_device.cpp @@ -680,6 +680,11 @@ k4a_result_t K4AROSDevice::fillColorPointCloud(const k4a::image& pointcloud_imag sensor_msgs::PointCloud2Modifier pcd_modifier(*point_cloud); pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); + point_cloud->height = pointcloud_image.get_height_pixels(); + point_cloud->width = pointcloud_image.get_width_pixels(); + point_cloud->row_step = pointcloud_image.get_width_pixels() * point_cloud->point_step; + point_cloud->data.resize(point_cloud->height * point_cloud->width * point_cloud->point_step); + sensor_msgs::PointCloud2Iterator iter_x(*point_cloud, "x"); sensor_msgs::PointCloud2Iterator iter_y(*point_cloud, "y"); sensor_msgs::PointCloud2Iterator iter_z(*point_cloud, "z"); @@ -688,13 +693,6 @@ k4a_result_t K4AROSDevice::fillColorPointCloud(const k4a::image& pointcloud_imag sensor_msgs::PointCloud2Iterator iter_g(*point_cloud, "g"); sensor_msgs::PointCloud2Iterator iter_b(*point_cloud, "b"); - pcd_modifier.resize(point_count); - - // Restore actual dimensions as pcd_modifier.resize(n) sets the cloud size to n x 1 - point_cloud->height = pointcloud_image.get_height_pixels(); - point_cloud->width = pointcloud_image.get_width_pixels(); - point_cloud->row_step = pointcloud_image.get_width_pixels() * point_cloud->point_step; - const int16_t* point_cloud_buffer = reinterpret_cast(pointcloud_image.get_buffer()); const uint8_t* color_buffer = color_image.get_buffer(); @@ -735,16 +733,14 @@ k4a_result_t K4AROSDevice::fillPointCloud(const k4a::image& pointcloud_image, se sensor_msgs::PointCloud2Modifier pcd_modifier(*point_cloud); pcd_modifier.setPointCloud2FieldsByString(1, "xyz"); - sensor_msgs::PointCloud2Iterator iter_x(*point_cloud, "x"); - sensor_msgs::PointCloud2Iterator iter_y(*point_cloud, "y"); - sensor_msgs::PointCloud2Iterator iter_z(*point_cloud, "z"); - - pcd_modifier.resize(point_count); - - // Set actual dimensions as pcd_modifier.resize(n) sets the cloud size to n x 1 point_cloud->height = pointcloud_image.get_height_pixels(); point_cloud->width = pointcloud_image.get_width_pixels(); point_cloud->row_step = pointcloud_image.get_width_pixels() * point_cloud->point_step; + point_cloud->data.resize(point_cloud->height * point_cloud->width * point_cloud->point_step); + + sensor_msgs::PointCloud2Iterator iter_x(*point_cloud, "x"); + sensor_msgs::PointCloud2Iterator iter_y(*point_cloud, "y"); + sensor_msgs::PointCloud2Iterator iter_z(*point_cloud, "z"); const int16_t* point_cloud_buffer = reinterpret_cast(pointcloud_image.get_buffer());