void buildOctoMap(const pcl::PointCloud<PointT> &cloud, OcTreeROS & tree) { pcl::PointCloud<PointT>::Ptr cloud_ptr(new pcl::PointCloud<PointT > (cloud)); pcl::PointCloud<PointT>::Ptr cloud_cam(new pcl::PointCloud<PointT > ()); int cnt =0; // find all the camera indices map<int,int> camera_indices; for (size_t i = 0; i < cloud.points.size(); ++i) { camera_indices[(int) cloud.points[i].cameraIndex] = 1; } // for every camera index .. apply filter and get the point cloud for (map<int,int>::iterator it = camera_indices.begin(); it != camera_indices.end();it++) { int ci = (*it).first; apply_camera_filter(*cloud_ptr,*cloud_cam,ci); // convert to pointXYZ format sensor_msgs::PointCloud2 cloud_blob; pcl::toROSMsg(*cloud_cam,cloud_blob); pcl::PointCloud<pcl::PointXYZ> xyzcloud; pcl::fromROSMsg(cloud_blob, xyzcloud); // find the camera co-ordinate VectorG cam_coordinates = originalFrames[ci]->getCameraTrans().getOrigin(); pcl::PointXYZ origin (cam_coordinates.v[0], cam_coordinates.v[1], cam_coordinates.v[2]); // insert to the tree tree.insertScan(xyzcloud,origin,-1,true); } }
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); }
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); }
al::perception::Entity fromEntityMsg (const al_msgs::Entity &msg_entity) { al::perception::Entity entity; if (msg_entity.cloud_object.data.size ()) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg (msg_entity.cloud_object, *cloud_ptr); entity.setCloudData (cloud_ptr); } entity.setFeatures (msg_entity.feature_vector); entity.setCollisionObject (msg_entity.collision_object); return entity; }
int main (int argc, char** argv) { sensor_msgs::PointCloud2 cloud_blob; pcl::PointCloud<PointT> cloud; if (pcl::io::loadPCDFile (argv[1], cloud_blob) == -1) { ROS_ERROR ("Couldn't read file test_pcd.pcd"); return (-1); } ROS_INFO ("Loaded %d data points from test_pcd.pcd with the following fields: %s", (int)(cloud_blob.width * cloud_blob.height), pcl::getFieldsList (cloud_blob).c_str ()); // Convert to the templated message type pcl::fromROSMsg (cloud_blob, cloud); pcl::PointCloud<PointT>::Ptr cloud_ptr (new pcl::PointCloud<PointT> (cloud)); pcl::PassThrough<PointT> pass; pass.setInputCloud (cloud_ptr); pass.filter (cloud); ROS_INFO ("Size after removing NANs : %d", (int)cloud.points.size()); ColorRGB color; vector<pcl::PointPLY> points (cloud.points.size()); for(size_t i = 0; i < cloud.points.size(); i++) { color.assignColor(cloud.points.at(i).rgb); points.at(i).x = cloud.points.at(i).x; points.at(i).y = cloud.points.at(i).y; points.at(i).z = cloud.points.at(i).z; points.at(i).r = color.getR(); points.at(i).g = color.getG(); points.at(i).b = color.getB(); } std::string fn (argv[1]); fn = fn.substr(0,fn.find('.')); fn = fn+ ".ply"; // pcl::io::savePCDFileASCII (fn, cloud); writePLY(fn,points); ROS_INFO ("Saved %d data points to ply file.", (int)cloud.points.size ()); return (0); }
int main(int argc, char** argv) { int scene_num = atoi(argv[2]); sensor_msgs::PointCloud2 cloud_blob; pcl::PointCloud<PointT> cloud; std::ofstream labelfile, nfeatfile, efeatfile; if (pcl::io::loadPCDFile(argv[1], cloud_blob) == -1) { ROS_ERROR("Couldn't read file test_pcd.pcd"); return (-1); } ROS_INFO("Loaded %d data points from test_pcd.pcd with the following fields: %s", (int) (cloud_blob.width * cloud_blob.height), pcl::getFieldsList(cloud_blob).c_str()); // convert to templated message type pcl::fromROSMsg(cloud_blob, cloud); pcl::PointCloud<PointT>::Ptr cloud_ptr(new pcl::PointCloud<PointT > (cloud)); pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT > ()); pcl::PointCloud<PointT>::Ptr cloud_seg(new pcl::PointCloud<PointT > ()); //pcl::PointCloud<PointXYZI>::Ptr cloud_seg (new pcl::PointCloud<PointXYZI> ()); std::vector<pcl::PointCloud<PointT> > segment_clouds; std::map<int,int> segment_num_index_map; pcl::PointIndices::Ptr segment_indices(new pcl::PointIndices()); // get segments std::set<int> myset; // find the max segment number int max_segment_num = 0; for (size_t i = 0; i < cloud.points.size(); ++i) { if ( cloud.points[i].label>0) { myset.insert(cloud.points[i].segment); } } set<int>::iterator it; SpectralProfile temp; //vector<SpectralProfile> spectralProfiles; for (it=myset.begin(); it!=myset.end(); it++) { apply_segment_filter_and_compute_HOG (*cloud_ptr,*cloud_seg,*it,temp); cerr<<*it<<"\t"<<cloud_seg->points[1].label<<"\t"<<cloud_seg->size()<<endl; //if (label!=0) cout << "segment: "<< seg << " label: " << label << " size: " << cloud_seg->points.size() << endl; } }
void getSegmentDistanceToBoundary( const pcl::PointCloud<PointT> &cloud , map<int,float> &segment_boundary_distance){ pcl::PointCloud<PointT>::Ptr cloud_rest(new pcl::PointCloud<PointT > ()); pcl::PointCloud<PointT>::Ptr cloud_cam(new pcl::PointCloud<PointT > ()); pcl::PointCloud<PointT>::Ptr cloud_seg(new pcl::PointCloud<PointT > ()); pcl::PointCloud<PointT>::Ptr cloud_ptr(new pcl::PointCloud<PointT > (cloud)); int cnt =0; // find all the camera indices// find the max segment number map<int,int> camera_indices; for (size_t i = 0; i < cloud.points.size(); ++i) { camera_indices[(int) cloud.points[i].cameraIndex] = 1; } // for every camera index .. apply filter anf get the point cloud for (map<int,int>::iterator it = camera_indices.begin(); it != camera_indices.end();it++) { int ci = (*it).first; apply_camera_filter(*cloud_ptr,*cloud_cam,ci); // find the segment list map<int,int> segments; for (size_t i = 0; i < cloud_cam->points.size(); ++i) { if( cloud_cam->points[i].label != 0) segments[(int) cloud_cam->points[i].segment] = 1; } for (map<int,int>::iterator it2 = segments.begin(); it2 != segments.end();it2++){ cnt++; int segnum = (*it2).first; apply_segment_filter(*cloud_cam,*cloud_seg,segnum); apply_notsegment_filter(*cloud_cam,*cloud_rest,segnum); float bdist = getDistanceToBoundary(*cloud_seg,*cloud_rest); map<int , float>::iterator segit = segment_boundary_distance.find(segnum); if(segit== segment_boundary_distance.end() || bdist> segment_boundary_distance[segnum] ) segment_boundary_distance[segnum] = bdist; // if(cnt == 1) outcloud = *cloud_seg; // else outcloud += *cloud_seg; } } //for(map<int,float>::iterator it = ) }
int LoadPCD::compute() { //for each selected filename for (int k = 0; k < m_filenames.size(); ++k) { QString filename = m_filenames[k]; sensor_msgs::PointCloud2 * pcd_sensor_cloud_in = loadSensorMessage(filename); boost::shared_ptr<sensor_msgs::PointCloud2> cloud_ptr_in = boost::make_shared<sensor_msgs::PointCloud2>(*pcd_sensor_cloud_in); sensor_msgs::PointCloud2::Ptr cloud_ptr (new sensor_msgs::PointCloud2); if (pcd_sensor_cloud_in->is_dense == false) //data may contain nans. Remove them { //now we need to remove nans pcl::PassThrough<sensor_msgs::PointCloud2> passFilter; passFilter.setInputCloud(cloud_ptr_in); passFilter.filter(*cloud_ptr); } else cloud_ptr = cloud_ptr_in; ccPointCloud* out_cloud = sm2ccConverter(cloud_ptr).getCCloud(); if (!out_cloud) return -31; QString cloud_name = QFileInfo(filename).baseName(); out_cloud->setName(cloud_name); QFileInfo fi(filename); QString containerName = QString("%1 (%2)").arg(fi.fileName()).arg(fi.absolutePath()); ccHObject* cloudContainer = new ccHObject(containerName); assert(out_cloud); cloudContainer->addChild(out_cloud); emit newEntity(cloudContainer); } return 1; }
TEST(NearestScanCostFunction, minDistanceToPointCloud) { wall_following::NearestScanCostFunction nearest_scan_cost_function; nearest_scan_cost_function.setMaxDistanceToScan(1.0); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>()); pcl::PointXYZ point_1; point_1.x = 0.0; point_1.y = 0.0; point_1.z = 0.0; pcl::PointXYZ point_2; point_2.x = 1.0; point_2.y = 1.0; point_2.z = 0.0; pcl::PointXYZ point_3; point_3.x = -10.0; point_3.y = 0.0; point_3.z = 0.0; cloud_ptr->points.push_back(point_1); cloud_ptr->points.push_back(point_2); nearest_scan_cost_function.initKdTree(cloud_ptr); double dist_1 = nearest_scan_cost_function.minDistanceToPointCloud(point_1.x, point_1.y); EXPECT_FLOAT_EQ(dist_1, 0.0); double dist_2 = nearest_scan_cost_function.minDistanceToPointCloud(point_2.x, point_2.y); EXPECT_FLOAT_EQ(dist_2, 0.0); double dist_3 = nearest_scan_cost_function.minDistanceToPointCloud(point_3.x, point_3.y); EXPECT_FLOAT_EQ(dist_3, 10.0); double dist_4 = nearest_scan_cost_function.minDistanceToPointCloud(-0.5, 0.0); EXPECT_FLOAT_EQ(dist_4, 0.5); }
int main() { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr1(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr segmented_cloud(new pcl::PointCloud<pcl::PointXYZ>); //load point cloud loadcloud(cloud_ptr); pcl::PointCloud <pcl::Normal>::Ptr normals(new pcl::PointCloud <pcl::Normal>); //calculate the normal of the point cloud. NormalCalculation(cloud_ptr,normals); //display point cloud. displaycloud(cloud_ptr); //display the normal of the point cloud. displayNormal(cloud_ptr, normals); //segmentation by region growing method. segmented_cloud=RegionGrowingSegmentation(cloud_ptr,normals); //display the segmented the cloud. displaycloud(segmented_cloud); return 0; }
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RGBDFrame::getPCLcloud(){ unsigned char * rgbdata = (unsigned char *)rgb.data; unsigned short * depthdata = (unsigned short *)depth.data; const unsigned int width = camera->width; const unsigned int height = camera->height; const double idepth = camera->idepth_scale; const double cx = camera->cx; const double cy = camera->cy; const double ifx = 1.0/camera->fx; const double ify = 1.0/camera->fy; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>); cloud->width = width; cloud->height = height; cloud->points.resize(width*height); for(unsigned int w = 0; w < width; w++){ for(unsigned int h = 0; h < height;h++){ int ind = h*width+w; double z = idepth*double(depthdata[ind]); if(z > 0){ pcl::PointXYZRGB p; p.x = (double(w) - cx) * z * ifx; p.y = (double(h) - cy) * z * ify; p.z = z; p.b = rgbdata[3*ind+0]; p.g = rgbdata[3*ind+1]; p.r = rgbdata[3*ind+2]; cloud->points[ind] = p; } } } pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne; ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT); ne.setMaxDepthChangeFactor(0.02f); ne.setNormalSmoothingSize(10.0f); ne.setInputCloud(cloud); ne.compute(*normals); pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGBNormal>); cloud_ptr->width = width; cloud_ptr->height = height; cloud_ptr->points.resize(width*height); for(unsigned int w = 0; w < width; w++){ for(unsigned int h = 0; h < height;h++){ int ind = h*width+w; pcl::PointXYZRGBNormal & p0 = cloud_ptr->points[ind]; pcl::PointXYZRGB p1 = cloud->points[ind]; pcl::Normal p2 = normals->points[ind]; p0.x = p1.x; p0.y = p1.y; p0.z = p1.z; p0.r = p1.r; p0.g = p1.g; p0.b = p1.b; p0.normal_x = p2.normal_x; p0.normal_y = p2.normal_y; p0.normal_z = p2.normal_z; } } return cloud_ptr; }
int main (int argc, char **argv) { std::cout << "main function" << std::endl; pcl::Grabber* interface; ros::NodeHandle *nh; ros::Subscriber sub; ros::Publisher clusterPub, normalPub, planePub,msgPub; bool spawnObject = true; K2G *k2g; processor freenectprocessor = OPENGL; boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB>> cloud; std::cout << "ros node initialized" << std::endl; ros::init(argc, argv, "hlpr_single_plane_segmentation",ros::init_options::NoSigintHandler); nh = new ros::NodeHandle("~"); // nh->getParam("segmentation/viz", viz_); // std::cout<<"viz is set to " << viz_ << endl; parsedArguments pA; if(parseArguments(argc, argv, pA, *nh) < 0) return 0; viz_ = pA.viz; ridiculous_global_variables::ignore_low_sat = pA.saturation_hack; ridiculous_global_variables::saturation_threshold = pA.saturation_hack_value; ridiculous_global_variables::saturation_mapped_value = pA.saturation_mapped_value; RansacSinglePlaneSegmentation multi_plane_app; pcl::PointCloud<PointT>::Ptr cloud_ptr (new pcl::PointCloud<PointT>); pcl::PointCloud<pcl::Normal>::Ptr ncloud_ptr (new pcl::PointCloud<pcl::Normal>); pcl::PointCloud<pcl::Label>::Ptr label_ptr (new pcl::PointCloud<pcl::Label>); boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer; multi_plane_app.initSegmentation(pA.seg_color_ind, pA.ecc_dist_thresh, pA.ecc_color_thresh); if(viz_) { viewer = cloudViewer(cloud_ptr); multi_plane_app.setViewer(viewer); } float workSpace[] = {-0.55,0.5,-0.2,0.45,0.3,2.0}; multi_plane_app.setWorkingVolumeThresholds(workSpace); msgPub = nh->advertise<hlpr_perception_msgs::SegClusters>(segOutRostopic,5); switch (pA.pc_source) { case 0: { std::cout << "Using ros topic as input" << std::endl; sub = nh->subscribe<sensor_msgs::PointCloud2>(pA.ros_topic, 1, cloud_cb_ros_); break; } case 1: { std::cout << "Using the openni device" << std::endl; interface = new pcl::OpenNIGrabber (); boost::function<void(const pcl::PointCloud<PointT>::ConstPtr&)> f = boost::bind (&cloud_cb_direct_,_1); boost::signals2::connection c = interface->registerCallback (f); interface->start (); break; } case 2: default: { std::cout << "Using kinect v2" << std::endl; freenectprocessor = static_cast<processor>(pA.freenectProcessor); k2g = new K2G(freenectprocessor, true); cloud = k2g->getCloud(); prev_cloud = cloud; gotFirst = true; break; } } signal(SIGINT, interruptFn); while (!interrupt && (!viz_ || !viewer->wasStopped ())) { if(pA.ros_node) { ros::spinOnce(); } if(viz_) viewer->spinOnce(20); if(!gotFirst) continue; int selected_cluster_index = -1; if(cloud_mutex.try_lock ()) { if(pA.pc_source == 2) { cloud = k2g->getCloud(); fake_cloud_cb_kinectv2_(cloud); } if(viz_ && pA.justViewPointCloud) { pcl::PointCloud<PointT>::Ptr filtered_prev_cloud(new pcl::PointCloud<PointT>(*prev_cloud)); multi_plane_app.preProcPointCloud(filtered_prev_cloud); if (!viewer->updatePointCloud<PointT> (filtered_prev_cloud, "cloud")) { viewer->addPointCloud<PointT> (filtered_prev_cloud, "cloud"); } selected_cluster_index = -1; } else { pcl::PointCloud<PointT>::CloudVectorType clusters; std::vector<pcl::PointCloud<pcl::Normal> > clusterNormals; Eigen::Vector4f plane; std::vector<size_t> clusterIndices; std::vector<std::vector<int>> clusterIndicesStore; selected_cluster_index = multi_plane_app.processOnce(prev_cloud,clusters,clusterNormals,plane,clusterIndicesStore, pA.pre_proc, pA.merge_clusters, viz_, pA.filterNoise); //true is for the viewer hlpr_perception_msgs::SegClusters msg; //msg.header.stamp = ros::Time::now(); // Pull out the cluster indices and put in msg for (int ti = 0; ti < clusterIndicesStore.size(); ti++){ hlpr_perception_msgs::ClusterIndex cluster_idx_msg; for (int j = 0; j < clusterIndicesStore[ti].size(); j++){ std_msgs::Int32 temp_msg; temp_msg.data = clusterIndicesStore[ti][j]; cluster_idx_msg.indices.push_back(temp_msg); } msg.cluster_ids.push_back(cluster_idx_msg); //clusterIndices.insert(clusterIndices.end(),clusterIndicesStore[ti].begin(), clusterIndicesStore[ti].end()); // For removing ALL cluster points } for(int i = 0; i < clusters.size(); i++) { sensor_msgs::PointCloud2 out; pcl::PCLPointCloud2 tmp; pcl::toPCLPointCloud2(clusters[i], tmp); pcl_conversions::fromPCL(tmp, out); msg.clusters.push_back(out); } for(int i = 0; i < clusterNormals.size(); i++) { sensor_msgs::PointCloud2 out; pcl::PCLPointCloud2 tmp; pcl::toPCLPointCloud2(clusterNormals[i], tmp); pcl_conversions::fromPCL(tmp, out); msg.normals.push_back(out); } std_msgs::Float32MultiArray planeMsg; planeMsg.data.clear(); for(int i = 0; i < 4; i++) planeMsg.data.push_back(plane[i]); //planePub.publish(planeMsg); msg.plane = planeMsg; msgPub.publish(msg); //clusterPub.publish(clusters[0]); //normalPub.publish(clusterNormals[0]); } cloud_mutex.unlock(); /*the z_thresh may result in wrong cluster to be selected. it might be a good idea to do * some sort of mean shift tracking, or selecting the biggest amongst the candidates (vs choosing the most similar color) * or sending all the plausible ones to c6 and decide there */ } if(selected_cluster_index < 0) continue; } if(pA.pc_source == 1) { interface->stop (); delete interface; } delete nh; ros::shutdown(); 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); }