Noetic and C++: concatenate point clouds using a deque

edit

Hi,

I'm trying to combine multiple point clouds in a sliding window fashion using a deque and then publish again the final accumulated (or concatenated) point cloud. I'm using the following code:

void combinePointClouds(pcl::PointCloud<PointType>::Ptr& current_cloud, pcl::PointCloud<PointType>::Ptr& new_msg_cloud,
                        int max_scans, std::deque<pcl::PointCloud<PointType>::Ptr>& point_cloud_deque,
                        unsigned long msg_timebase_ns, ros::Publisher& publisher)
{

    // Add the new point cloud to the deque
    point_cloud_deque.push_front(new_msg_cloud);

    // Clear the current point cloud
    current_cloud->clear();

    // Add the points from the last max_scans point clouds to the current point cloud
     for (auto it = point_cloud_deque.begin(); it != point_cloud_deque.end(); it++)
     {
         *current_cloud += **it;
     }

    // Publish the point cloud
    publishPointCloud(current_cloud, msg_timebase_ns, publisher); 

    // If the deque has more than max_scans elements, remove the oldest element
    if (point_cloud_deque.size() >= max_scans)
    {
        point_cloud_deque.pop_back();
    }
}

void publishPointCloud(pcl::PointCloud<PointType>::Ptr& pcl_ptr, unsigned long msg_timebase_ns, ros::Publisher& publisher)
{
    sensor_msgs::PointCloud2 pcl_ros_msg;
    pcl::toROSMsg(*pcl_ptr.get(), pcl_ros_msg);
    pcl_ros_msg.header.stamp.fromNSec(msg_timebase_ns);
    pcl_ros_msg.header.frame_id = "livox";
    publisher.publish(pcl_ros_msg);
}

I checked the size of the point cloud right before being published and it's correct (it contains as many points as I would expect concatenating the point clouds). I also echoed the topic where I publish the point cloud and the total number of points correspond to the size of the point cloud before being published.

When I visualize the final point cloud using RViz I can only see the points from the last scanned point cloud. For instance, if the last scanned point cloud has 8 points and accumulating the last 3 scans I expect to see 24, the size of the final point cloud is 24 but RViz shows only the 8 points of the last scan.

If I removed the for loop over the deque as well as

current_cloud->clear();

and use directly the command

*current_cloud += *new_msg_cloud;

then the point clouds are accumulated one scan at a time.

I also saved the accumulated point clouds in a pcd file in the last "if" to make sure that the deque contained at least N scans. When I visualize the pcd file it shows again only the point cloud related to the last scan. I don't understand why the point clouds stored in the deque don't seem to concatenate but to overwrite.

I also tried using a list rather than a deque and the result didn't change.