int Utils::combinePointCloud(const CloudPtr &cloud1, const CloudPtr &cloud2, const CloudPtr &combinedCloud, float leaf_size)
{
CloudPtr cloudp(new Cloud());
(*cloudp) = (*cloud1) + (*cloud2);
if (leaf_size > 0)
downSamplePointCloud(cloudp, combinedCloud, leaf_size);
else
(*combinedCloud) = (*cloudp);
return 0;
}
int Utils::downSamplePointCloud(const CloudPtr &cloud_in, const CloudPtr &cloud_out, float leaf_size)
{
CloudPtr cloudp(new Cloud());
(*cloudp) = (*cloud_in);
pcl::VoxelGrid<Point> sor;
sor.setInputCloud(cloudp);
sor.setLeafSize(leaf_size, leaf_size, leaf_size);
sor.filter(*cloud_out);
std::cout << (*cloudp).points.size() << "->" << (*cloud_out).points.size() << std::endl;
return 0;
}
int Utils::combinePointCloud(const std::vector<CloudPtr> clouds, const CloudPtr &combinedCloud, float leaf_size)
{
CloudPtr cloudp(new Cloud());
for (auto i = clouds.begin(); i != clouds.end(); i++)
(*cloudp) += (**i);
if (leaf_size > 0)
downSamplePointCloud(cloudp, combinedCloud, leaf_size);
else
(*combinedCloud) = (*cloudp);
return 0;
}
int main(int argc, char** argv)
{
if (argc < 2)
{
PCL_ERROR("Usage: %s gt.pcd\n", argv[0]);
exit(1);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudp (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> (argv[1], *cloudp) == -1)
{
PCL_ERROR("Failed to load file %s\n", argv[1]);
exit(1);
}
else
PCL_WARN("Loaded PCD %s\n", argv[1]);
pcl::ModelCoefficients::Ptr coeff (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inlier = RansacMethod(cloudp, coeff);
// RaycostMethod(cloudp, center);
printf("%lf %lf %lf %lf\n", coeff->values[0], coeff->values[1], coeff->values[2], coeff->values[3]);
#if 0
// Separate and colorize inliers and outliers
pcl::PointCloud<pcl::PointXYZRGB>::Ptr clr_cloud = ColorizeCloud(cloudp, inlier);
// Display the final result
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer = SetupViewer(clr_cloud, coeff);
while( !viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
#endif
return 0;
}
