Problem
Related to Issue 163
On the latest melodic branch, when subscribing to the /points2 pointcloud topic, the received sensor_msgs::PointCloud2 messages are unordered (i.e. height = 1).
In my testing for example, when point_cloud_in_depth_frame=false and color_resolution=720P, the message will have a height of 1 and width of 921,600 (exactly 720 * 1280). It seems to be publishing an ordered point cloud as if it were unordered.
This is inconvenient - for instance when using pcl::fromROSMsg() to convert the message to a pcl::PointCloud, the point cloud cannot be grid indexed because it's unordered. This can be worked around by writing a function to manually grid index a point with pointer arithmetic, but certainly not ideal.
Requested Solution
Investigating why the sensor_msgs::PointCloud2 message are published unordered in the first place, took me to K4AROSDevice::fillColorPointCloud().
The line pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); sets the message's pointcloud fields and also resizes the actual data array. No issue here. Refer to line 198 in the ROS implementation.
However a few lines down pcd_modifier.resize(point_count); is called, which I believe is redundant, and also assigns height to 1 and assigns width accordingly, making it unordered. Refer to line 116 in the ROS implementation.
Removing this line leads to publishing an ordered point cloud message with the expected height and width.
Tested and working on my fork.
Additional context
I believe same issue applies to K4AROSDevice::fillPointCloud().
Problem
Related to Issue 163
On the latest melodic branch, when subscribing to the
/points2pointcloud topic, the receivedsensor_msgs::PointCloud2messages are unordered (i.e. height = 1).In my testing for example, when
point_cloud_in_depth_frame=falseandcolor_resolution=720P, the message will have a height of 1 and width of 921,600 (exactly 720 * 1280). It seems to be publishing an ordered point cloud as if it were unordered.This is inconvenient - for instance when using
pcl::fromROSMsg()to convert the message to apcl::PointCloud, the point cloud cannot be grid indexed because it's unordered. This can be worked around by writing a function to manually grid index a point with pointer arithmetic, but certainly not ideal.Requested Solution
Investigating why the
sensor_msgs::PointCloud2message are published unordered in the first place, took me toK4AROSDevice::fillColorPointCloud().The line
pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");sets the message's pointcloud fields and also resizes the actual data array. No issue here. Refer to line 198 in the ROS implementation.However a few lines down
pcd_modifier.resize(point_count);is called, which I believe is redundant, and also assigns height to 1 and assigns width accordingly, making it unordered. Refer to line 116 in the ROS implementation.Removing this line leads to publishing an ordered point cloud message with the expected height and width.
Tested and working on my fork.
Additional context
I believe same issue applies to
K4AROSDevice::fillPointCloud().