Merge 2 Pointcloud2s

asked 2020-06-04 08:30:50 -0600

TomSon gravatar image

Hi, I would like to assemble two pointcloud2 : one from a 3D Lidar and the other from a RGBD camera.

First, I thought about point_cloud2_assembler from the package laser_assembler but I don't think it does what I want. I also need that the merged pointcloud to be published more like a topic and not by a service call. Both sensors use their proper frame also. I would like to be transformed to base_link.

Does anyone know how to do that ?

edit retag flag offensive close merge delete

Comments

Hi @TomSon, To avoid an xy-problem, can you provide more background info about what you want to achieve with that?

Weasfas gravatar image Weasfas  ( 2020-06-05 04:41:30 -0600 )edit

Hi @Weasfas, I would like to merge those two pointcloud and give it as input to rtabmap. To optimize a little, I can transform from one to another sensor to avoid 2 transforms to base_link. I could also change the pointcloud from the 3Dlidar to Laserscan but it would mean a loss of sensor data just for a input compatibility.

TomSon gravatar image TomSon  ( 2020-06-05 04:53:11 -0600 )edit

I understand, well, since you are dealing with pointclouds I assume you are using PCL. You can merge two pointcloud by using the += operator.

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

PointCloud::Ptr acc_cloud(new PointCloud);
acc_cloud->header.frame_id = "frame_id";

*acc_cloud+= *input_cloud;

Also I do not know how ou are transforming the pointcloud but you can try the Eigen approach:

Eigen::Affine3d relative_transform = ...;
PointCloud::Ptr transformed_cloud(new PointCloud());
pcl::transformPointCloud(*cloud_in, *transformed_cloud, relative_transform);

Hope that helps.

Weasfas gravatar image Weasfas  ( 2020-06-05 06:07:24 -0600 )edit

Thanks for your comment, that's food for thought. I didn't want to make a conversion to PCL to avoid loss in performance. I thought to use sensor_msg\pointcloud2 function and tf ones, same as laser_assembler_node.

TomSon gravatar image TomSon  ( 2020-06-05 06:25:15 -0600 )edit

Mmm, I do not know if the is a huge impact in the performance, as I see either you cope with the permance of pcl functions or, with the performance of STL functions. If you do not want to use PCL a solution would be using STL vector functions to concatenate vector points as well as the channels if you pointclouds have them.

I have always used all my pointcloud callbacks with PCL types and I never noticed an impact in performance:

void cloud_callback(const PointCloud::Ptr& cloud_msg)
{
 ...
}

But I will say that, it depends on you and what you think is most suitable for your implementation.

Weasfas gravatar image Weasfas  ( 2020-06-05 07:42:11 -0600 )edit

I try the same code as nodelet laserscan_to_pointcloud, I can transform to the frame but I need to concate both pointcloud on a new one and, then publish it. Useful code :)

TomSon gravatar image TomSon  ( 2020-06-05 08:53:41 -0600 )edit

Well, since that code is using message_filter I assume you will be dealing with sensor_msgs/PointCloud2 so a std::copy of the vector points will do the trick.

Weasfas gravatar image Weasfas  ( 2020-06-06 05:23:06 -0600 )edit

Thanks for the advice, I have the main structure of the code. I hope I can test it today.

TomSon gravatar image TomSon  ( 2020-06-08 02:41:50 -0600 )edit