int
main (int, char** av)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_no_nans (new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_segmented (new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PCDWriter writer;
if (pcl::io::loadPCDFile(av[1], *cloud_ptr)==-1)
{
std::cout << "Couldn't find the file " << av[1] << std::endl;
return -1;
}
std::cout << "Loaded cloud " << av[1] << " of size " << cloud_ptr->points.size() << std::endl;
// Remove the nans
cloud_ptr->is_dense = false;
cloud_no_nans->is_dense = false;
std::vector<int> indices;
pcl::removeNaNFromPointCloud (*cloud_ptr, *cloud_no_nans, indices);
std::cout << "Removed nans from " << cloud_ptr->points.size () << " to " << cloud_no_nans->points.size () << std::endl;
// Estimate the normals
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud_no_nans);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_n (new pcl::search::KdTree<pcl::PointXYZ>());
ne.setSearchMethod (tree_n);
ne.setRadiusSearch (0.03);
ne.compute (*cloud_normals);
std::cout << "Normals are computed and size is " << cloud_normals->points.size () << std::endl;
// Region growing
pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> rg;
rg.setSmoothModeFlag (false); // Depends on the cloud being processed
rg.setInputCloud (cloud_no_nans);
rg.setInputNormals (cloud_normals);
std::vector <pcl::PointIndices> clusters;
rg.extract (clusters);
cloud_segmented = rg.getColoredCloud ();
// Writing the resulting cloud into a pcd file
std::cout << "No of segments done is " << clusters.size () << std::endl;
writer.write<pcl::PointXYZRGB> ("segment_result.pcd", *cloud_segmented, false);
return (0);
}
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_no_nans (new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_segmented (new pcl::PointCloud<pcl::PointXYZRGB>());
struct timeval tpstart,tpend;
float timeuse;
pcl::PCDWriter writer;
if (pcl::io::loadPCDFile(argv[1], *cloud_ptr)==-1)
{
std::cout << "Couldn't find the file " << argv[1] << std::endl;
return -1;
}
std::cout << "Loaded cloud " << argv[1] << " of size " << cloud_ptr->points.size() << std::endl;
// Remove the nans
cloud_ptr->is_dense = false;
cloud_no_nans->is_dense = false;
std::vector<int> indices;
pcl::removeNaNFromPointCloud (*cloud_ptr, *cloud_no_nans, indices);
std::cout << "Removed nans from " << cloud_ptr->points.size () << " to " << cloud_no_nans->points.size () << std::endl;
gettimeofday(&tpstart,NULL);
// Estimate the normals
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud_no_nans);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_n (new pcl::search::KdTree<pcl::PointXYZ>());
ne.setSearchMethod (tree_n);
ne.setKSearch(30);
//ne.setRadiusSearch (0.03);
ne.compute (*cloud_normals);
std::cout << "Normals are computed and size is " << cloud_normals->points.size () << std::endl;
// Region growing
pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> rg;
rg.setMinClusterSize (50);
rg.setSmoothModeFlag (true); // Depends on the cloud being processed
rg.setSmoothnessThreshold (10*M_PI/180);
rg.setResidualTestFlag (true);
rg.setResidualThreshold (0.005);
rg.setCurvatureTestFlag (true);
rg.setCurvatureThreshold (0.008);
rg.setNumberOfNeighbours (30);
rg.setInputCloud (cloud_no_nans);
rg.setInputNormals (cloud_normals);
std::vector <pcl::PointIndices> clusters;
rg.extract (clusters);
cloud_segmented = rg.getColoredCloud ();
cloud_segmented->width = cloud_segmented->points.size();
cloud_segmented->height = 1;
gettimeofday(&tpend,NULL);
timeuse=1000000*(tpend.tv_sec-tpstart.tv_sec) + tpend.tv_usec-tpstart.tv_usec;
timeuse/=1000000;
std::cout << "Segmentation time: " << timeuse << std::endl;
std::cout << "Number of segments done is " << clusters.size () << std::endl;
writer.write<pcl::PointXYZRGB> ("segment_result.pcd", *cloud_segmented, false);
std::cout << "Number of points in the segments: " << cloud_segmented->size() << std::endl;
pcl::visualization::PCLVisualizer viewer ("Detected planes with Pseudo-color");
viewer.setBackgroundColor (1.0, 1.0, 1.0);
viewer.addPointCloud (cloud_segmented, "cloud segmented", 0);
viewer.addCoordinateSystem ();
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud segmented");
viewer.spin();
// Writing the resulting cloud into a pcd file
return (0);
}
