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

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.

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.

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.

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.

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 :)

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.

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

I may have an issue here, I want to keep the pointclouds the same as there are but from a different frame. I try a default transform but all the points are moved. What operation should I use to do that ? Just a copy and a target frame rename ? The pointcloud struct is global or relative to the frame ?

Hi @TomSon,

If you have a problem not related to the main post, can you generate another post as the comunity guidelines says? Would be better to address it in another one. You can comment here the link. Regards.

Nevermind, for a moment I thought that the transform would change the position of the pointcloud. Could you check if it's correct ? https://github.com/tomlogan501/pointcloud_merger I will add an integration test to verify with PCL as an arbitrary

@TomSon, if you apply the transform to your pointcloud this should change the position of each point within the pointcloud, if not, either you are transforming to the same frame or the transformation is not being performed correctly. For me, the code is fine, and you should obtain an accumaled pointcloud in the target_frame_ frame.

Hi @Weasfas, Well then I should not do a transform and just iterate in the copy ?

Well,you can perform the transformation to your wanted frame for both incoming clouds and add them into an accumulated one, but I still do not know why you want to transform. Let us assume you want to transform because you want to store the base_footprint projection of the pointcloud... in that case, you should transform first each independent pointcloud to the base_footprint and then add both vector points to an accumulated one, finally you will end with a new poincloud storing both incoming pointclouds points with each point relative to the base_footprint frame. As I said I have more experience with pcl as with raw sensor_msgs but I think that is the way to go.

I got a strange behavior with the display on rviz, I add some chrono time measurement to be sure but changes are really slow to display. Is there some race condition I might have introduce ?

Well, I guess it depends on the cloud size. Rviz seems to struggle when rendering big pointclouds, that may be the cause of your speed problem.

Hi @Weasfas, seems that I got a bug for big pointclouds, can you test it from your side with 2 lidars ? One of the pointcloud seems to got its sources updated or cleared, i don't figured it out :(

Yes, the problem with big pointcloud is the processing time needed for each one, if you have a message filter and the processing cannot keep up with each incoming set of clouds you can end losing data or mergin points that do not belong to the cloud you are already processing. The thing is that when dealing with pointers seems very hard to know what is happening, you may want to use gdb or process step by step, printing states and data structures to see what is happening, maybe your mutex/semaphores are not working properly or the filter just drops the messages because of its header time. Since I have never worked with this approach I cannot tell what could be happening without some deep test of the code. I always work with PCL and for me it is easier comparing with what you are trying to do with the code.

I may try to convert the code to PCL but I didn't want to get a dependancy to PCL in the source code, just in the tests.

Also I can deal with performance issue by transforming one pointcloud to another to save CPU. In a nodelet, is there a good use of multi-threading ? It's the default behavior

Well, transforming the cloud into another is not necesary a practise from multithreading, you can just convert it to a pointcloud of height 1 and work with it like a one dimensional set of points in a vector. I am not an expert in that particular topic and maybe if you generate another post with performance enhance question you will find usefull help here.