Noetic and C++: concatenate point clouds using a deque
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.