Fix publishing ordered pointcloud messages as unordered

[rockmasstech]

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().

[christian-rauch]

Can you send a PR with your changes so they can be reviewed and properly integrated?

[ndaley7]

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().

Thank you for this fix!