int main (int, char **argv) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ> ()); pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ()); pcl::PCDWriter writer; if (pcl::io::loadPCDFile<pcl::PointXYZ> (argv[1], *cloud_ptr) == -1) { std::cout<<"Couldn't read the file "<<argv[1]<<std::endl; return (-1); } std::cout << "Loaded pcd file " << argv[1] << " with " << cloud_ptr->points.size () << std::endl; // Normal estimation pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne; ne.setInputCloud (cloud_ptr); 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 << "Estimated the normals" << std::endl; // Creating the kdtree object for the search method of the extraction boost::shared_ptr<pcl::KdTree<pcl::PointXYZ> > tree_ec (new pcl::KdTreeFLANN<pcl::PointXYZ> ()); tree_ec->setInputCloud (cloud_ptr); // Extracting Euclidean clusters using cloud and its normals std::vector<int> indices; std::vector<pcl::PointIndices> cluster_indices; const float tolerance = 0.5f; // 50cm tolerance in (x, y, z) coordinate system const double eps_angle = 5 * (M_PI / 180.0); // 5degree tolerance in normals const unsigned int min_cluster_size = 50; pcl::extractEuclideanClusters (*cloud_ptr, *cloud_normals, tolerance, tree_ec, cluster_indices, eps_angle, min_cluster_size); std::cout << "No of clusters formed are " << cluster_indices.size () << std::endl; // Saving the clusters in seperate pcd files int j = 0; for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>); for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++) cloud_cluster->points.push_back (cloud_ptr->points[*pit]); cloud_cluster->width = static_cast<uint32_t> (cloud_cluster->points.size ()); cloud_cluster->height = 1; cloud_cluster->is_dense = true; std::cout << "PointCloud representing the Cluster using xyzn: " << cloud_cluster->points.size () << " data points." << std::endl; std::stringstream ss; ss << "./cloud_cluster_" << j << ".pcd"; writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); j++; } return (0); }
inline void filter_depth_discontinuity( const pcl::PointCloud<PointT> &in, pcl::PointCloud<PointT> &out, int neighbors = 2, float threshold = 0.3, float distance_min = 1, float distance_max = 300, float epsilon = 0.5 ) { //std::cout << "neigbors " << neighbors << " epsilon " << epsilon << " distance_max " << distance_max <<std::endl; boost::shared_ptr<pcl::search::KdTree<PointT> > tree_n( new pcl::search::KdTree<PointT>() ); tree_n->setInputCloud(in.makeShared()); tree_n->setEpsilon(epsilon); for(int i = 0; i< in.points.size(); ++i){ std::vector<int> k_indices; std::vector<float> square_distance; if ( in.points.at(i).z > distance_max || in.points.at(i).z < distance_min) continue; //Position in image is known tree_n->nearestKSearch(in.points.at(i), neighbors, k_indices, square_distance); //std::cout << "hier " << i << " z " << in.points.at(i).z <<std::endl; #ifdef USE_SQUERE_DISTANCE const float point_distance = distance_to_origin<PointT>(in.points.at(i)); #else const float point_distance = in.points.at(i).z; #endif float value = 0; //point_distance; unsigned int idx = 0; for(int n = 0; n < k_indices.size(); ++n){ #ifdef USE_SQUERE_DISTANCE float distance_n = distance_to_origin<PointT>(in.points.at(k_indices.at(n))); #else float distance_n = in.points.at(k_indices.at((n))).z; #endif if(value < distance_n - point_distance){ idx = k_indices.at(n); value = distance_n - point_distance; } } if(value > threshold){ out.push_back(in.points.at(i)); out.at(out.size()-1).intensity = sqrt(value); } } }
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, char** argv) { std::string filename = argv[1]; std::cout << "Reading " << filename << std::endl; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>); if(pcl::io::loadPCDFile<pcl::PointXYZ> (filename, *cloud_xyz) == -1) // load the file { PCL_ERROR ("Couldn't read file"); return -1; } std::cout << "points: " << cloud_xyz->points.size () <<std::endl; // Parameters for sift computation const float min_scale = 0.01f; const int n_octaves = 3; const int n_scales_per_octave = 4; const float min_contrast = 0.001f; // Estimate the normals of the cloud_xyz pcl::NormalEstimation<pcl::PointXYZ, pcl::PointNormal> ne; pcl::PointCloud<pcl::PointNormal>::Ptr cloud_normals (new pcl::PointCloud<pcl::PointNormal>); pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_n(new pcl::search::KdTree<pcl::PointXYZ>()); ne.setInputCloud(cloud_xyz); ne.setSearchMethod(tree_n); ne.setRadiusSearch(0.2); ne.compute(*cloud_normals); // Copy the xyz info from cloud_xyz and add it to cloud_normals as the xyz field in PointNormals estimation is zero for(size_t i = 0; i<cloud_normals->points.size(); ++i) { cloud_normals->points[i].x = cloud_xyz->points[i].x; cloud_normals->points[i].y = cloud_xyz->points[i].y; cloud_normals->points[i].z = cloud_xyz->points[i].z; } // Estimate the sift interest points using normals values from xyz as the Intensity variants pcl::SIFTKeypoint<pcl::PointNormal, pcl::PointWithScale> sift; pcl::PointCloud<pcl::PointWithScale> result; pcl::search::KdTree<pcl::PointNormal>::Ptr tree(new pcl::search::KdTree<pcl::PointNormal> ()); sift.setSearchMethod(tree); sift.setScales(min_scale, n_octaves, n_scales_per_octave); sift.setMinimumContrast(min_contrast); sift.setInputCloud(cloud_normals); sift.compute(result); std::cout << "No of SIFT points in the result are " << result.points.size () << std::endl; /* // Copying the pointwithscale to pointxyz so as visualize the cloud pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp (new pcl::PointCloud<pcl::PointXYZ>); copyPointCloud(result, *cloud_temp); std::cout << "SIFT points in the cloud_temp are " << cloud_temp->points.size () << std::endl; // Visualization of keypoints along with the original cloud pcl::visualization::PCLVisualizer viewer("PCL Viewer"); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> keypoints_color_handler (cloud_temp, 0, 255, 0); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_color_handler (cloud_xyz, 255, 0, 0); viewer.setBackgroundColor( 0.0, 0.0, 0.0 ); viewer.addPointCloud(cloud_xyz, cloud_color_handler, "cloud"); viewer.addPointCloud(cloud_temp, keypoints_color_handler, "keypoints"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints"); while(!viewer.wasStopped ()) { viewer.spinOnce (); } */ 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); }