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]; }