fromROSMsg optimized (almost 2X speed)

[facontidavide]

Hi,

I can not count the number of times some colleague complained about pcl::fromROSMsg consuming large amont of CPU!

After looking at the implementation, I realized that we are copying the data twice, using the pcl::PCLPointCloud2 pcl_pc2 as intermediate data structure.

First of all, let me tell you that I HATE copy and pasting and that I absolutely agree that code readability come first.

BUT, this function is the bread and butter of every ROS + PCL application, and the amount of CPU improvement is really too much to be ignored!

Please see my benchmark comparison here:

Cheers!

[facontidavide]

Please note that I am applying this to foxy, but it can be ported to all the other branches, including ROS1

[SteveMacenski]

I couldn’t agree more, I’ll take a look at this on Monday

[mvieth]

The speedup is obviously very nice, but as you said, duplicating the code of pcl::fromPCLPointCloud2 is not a good idea. Future changes and improvements to that function would not reach the ROS users. I also noticed that you didn't copy the function code from the master branch? Perhaps we can find some other way to achieve this speedup? Is the function moveFromROSMsg not an option for you? Or perhaps we consider solving this on the PCL side, by creating a new templated function fromPCLPointCloud2 (const T& msg, pcl::PointCloud<PointT>& cloud), which receives a msg which can either be a sensor_msgs::msg::PointCloud2 or a pcl::PCLPointCloud2 (fields are the same)? Not yet 100% sure this works, but might be an idea?

[facontidavide]

1. First of all, I think anybody agree here that the CPU waster is absolutely unacceptable.

Perhaps we can find some other way to achieve this speedup? Is the function moveFromROSMsg not an option for you?

2. We can not move the data because the ROS message is passed by const reference.

Or perhaps we consider solving this on the PCL side, by creating a new templated function fromPCLPointCloud2 (const T& msg, pcl::PointCloud<PointT>& cloud)

3. I thought about that and it is an option. It can be technically done, but I leave that to you maintainers.

I also noticed that you didn't copy the function code from the master branch?

4. I can fix that, if this is an issue. You will need to port this changes to all your branched, though.

[SteveMacenski]

@mvieth I'm more than happy to have the fix be internal to PCL if you like, that would seem to be the best solution to reduce code duplication.

[mvieth]

@facontidavide Would you like to open a pull request at https://github.com/PointCloudLibrary/pcl with a first draft? Then we can discuss everything further there and decide whether that's the way we want to go :slightly_smiling_face:

[vinnnyr]

Just so I can keep up to date -- is there any movement on this? is there a pcl pr that we should link to this one? no worries if not.

[SteveMacenski]

Agreed! This would be great to get in

[mvieth]

So I tested my idea from above a bit (creating a new templated function fromPCLPointCloud2 (const T& msg, pcl::PointCloud<PointT>& cloud) in PCL), but the time stamp in the header is a problem. ROS1 and ROS2 have different types for the time stamp, and if we would want to handle both on the PCL side, it would get quite ugly. Instead we could add a function template <typename PointT> void fromPCLPointCloud2 (const pcl::PCLPointCloud2& msg, pcl::PointCloud<PointT>& cloud, const MsgFieldMap& field_map, const std::uint8_t* msg_data) to PCL, which would use msg_data instead of msg.data. And then fromROSMsg would be:

pcl::PCLPointCloud2 pcl_pc2;
pcl_conversions::copyPointCloud2MetaData(cloud, pcl_pc2); // Like pcl_conversions::toPCL, but does not copy the binary data
pcl::MsgFieldMap field_map;
pcl::createMapping<T> (pcl_pc2.fields, field_map);
pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud, field_map, &cloud.data[0]);

This would mean no copying of code from PCL to perception_pcl, and still only copying the binary point data once. We could add the new function to PCL with release 1.13.1, and then everyone who uses PCL >= 1.13.1 and an up-to-date perception_pcl would automatically have the speedup. Thoughts welcome :smile:

[SteveMacenski]

Looks good to me!

[SteveMacenski]

Other PR supersedes