Skip to content
This repository has been archived by the owner on Jul 26, 2024. It is now read-only.

Fix Unordered Point Clouds #241

Open
wants to merge 3 commits into
base: melodic
Choose a base branch
from
Open

Fix Unordered Point Clouds #241

Changes from 1 commit
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
838bbc1
fixed wrong point clound size info
exo-core Mar 2, 2022
e05426e
resizing point cloud data after initializing iterators was a bad idea
exo-core Oct 24, 2022
e886156
Merge branch 'melodic' into fix_unordered_point_cloud
exo-core Sep 21, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 10 additions & 4 deletions src/k4a_ros_device.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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<const int16_t*>(pointcloud_image.get_buffer());
const uint8_t* color_buffer = color_image.get_buffer();

Expand Down Expand Up @@ -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;

Expand All @@ -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<const int16_t*>(pointcloud_image.get_buffer());

for (size_t i = 0; i < point_count; i++, ++iter_x, ++iter_y, ++iter_z)
Expand Down