sensor_msgs::PointCloud2 mergeVectorOfClouds(const std::vector<sensor_msgs::PointCloud2> &clouds)
{
int n_points = clouds[0].height * clouds[0].width;
int n_clouds = clouds.size();
//all the indices
std::vector<int> indices(n_points);
for (int i = 0; i < n_points; ++i)
{
indices.at(i) = i;
}
sensor_msgs::PointCloud2::Ptr sm_cloud (new sensor_msgs::PointCloud2); //out cloud
sensor_msgs::PointCloud2::Ptr sm_tmp (new sensor_msgs::PointCloud2); //temporary cloud
*sm_cloud = clouds[0];
//now loop on scalar fields and merge them
for (int i = 1; i < n_clouds; ++i)
{
pcl::copyPointCloud(*sm_cloud, indices, *sm_tmp);
pcl::concatenateFields(*sm_tmp, clouds[i], *sm_cloud);
}
return *sm_cloud;
}
PCLCloud mergeVectorOfClouds(std::vector<PCLCloud> &clouds)
{
pcl::uint32_t n_points = clouds[0].height * clouds[0].width;
size_t n_clouds = clouds.size();
if (clouds.empty())
return PCLCloud();
//all the indexes
std::vector<int> indexes;
{
try
{
indexes.resize(n_points);
}
catch(std::bad_alloc)
{
//not enough memory
return PCLCloud();
}
for (pcl::uint32_t i = 0; i < n_points; ++i)
{
indexes[i] = i;
}
}
//now loop on scalar fields and merge them
{
for (size_t i = 1; i < n_clouds; ++i)
{
PCLCloud::Ptr sm_tmp (new PCLCloud); //temporary cloud
pcl::copyPointCloud(clouds[0], indexes, *sm_tmp);
pcl::concatenateFields(*sm_tmp, clouds[i], clouds[0]);
}
}
return clouds[0];
}
