void geonDetector::initialize() { totalCount = 0; pcl::PointCloud<PointT>::Ptr cloud1 (new pcl::PointCloud<PointT>()); pcl::PointCloud<PointT>::Ptr cloud_filtered1 (new pcl::PointCloud<PointT>()); pcl::PointCloud<PointT>::Ptr cloud_filtered21 (new pcl::PointCloud<PointT>()); pcl::PointCloud<pcl::Normal>::Ptr cloud_normals1 (new pcl::PointCloud<pcl::Normal>()); pcl::PointCloud<pcl::Normal>::Ptr cloud_normals21 (new pcl::PointCloud<pcl::Normal>()); pcl::ModelCoefficients::Ptr coefficients1 (new pcl::ModelCoefficients()); pcl::PointIndices::Ptr geonIndices1 (new pcl::PointIndices()); pcl::search::KdTree<PointT>::Ptr tree2 (new pcl::search::KdTree<PointT>()); this->cloud = cloud1; this->cloud_filtered = cloud_filtered1; this->cloud_normals = cloud_normals1; this->cloud_filtered2 = cloud_filtered21; this->cloud_normals2 = cloud_normals21; this->coefficients = coefficients1; this->geonIndices = geonIndices1; this->tree = tree2; }
void PlaneFinder::pcCallback(const sensor_msgs::PointCloud2::ConstPtr & pc) { pcl::PCLPointCloud2::Ptr pcl_pc(new pcl::PCLPointCloud2); pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ()); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered1 (new pcl::PointCloud<pcl::PointXYZRGB>()); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered2 (new pcl::PointCloud<pcl::PointXYZRGB>()); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered3 (new pcl::PointCloud<pcl::PointXYZRGB>()); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>()); pcl::PCLPointCloud2::Ptr cloud_out (new pcl::PCLPointCloud2 ()); sensor_msgs::PointCloud2 pc2; double height = -0.5; // Transformation into PCL type PointCloud2 pcl_conversions::toPCL(*(pc), *(pcl_pc)); ////////////////// // Voxel filter // ////////////////// pcl::VoxelGrid<pcl::PCLPointCloud2> sor; sor.setInputCloud (pcl_pc); sor.setLeafSize (0.01f, 0.01f, 0.01f); sor.filter (*cloud_filtered); // Transformation into ROS type //pcl::toPCLPointCloud2(*(cloud_filtered2), *(cloud_out)); //pcl_conversions::moveFromPCL(*(cloud_filtered), pc2); //debug2_pub.publish(pc2); // Transformation into PCL type PointCloud<pcl::PointXYZRGB> pcl::fromPCLPointCloud2(*(cloud_filtered), *(cloud_filtered1)); if(pcl_ros::transformPointCloud("map", *(cloud_filtered1), *(cloud_filtered2), tf_)) { //////////////////////// // PassThrough filter // //////////////////////// pcl::PassThrough<pcl::PointXYZRGB> pass; pass.setInputCloud (cloud_filtered2); pass.setFilterFieldName ("x"); pass.setFilterLimits (-0.003, 0.9); //pass.setFilterLimitsNegative (true); pass.filter (*cloud_filtered2); pass.setInputCloud (cloud_filtered2); pass.setFilterFieldName ("y"); pass.setFilterLimits (-0.5, 0.5); //pass.setFilterLimitsNegative (true); pass.filter (*cloud_filtered2); ///////////////////////// // Planar segmentation // ///////////////////////// pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers (new pcl::PointIndices); // Create the segmentation object pcl::SACSegmentation<pcl::PointXYZRGB> seg; // Optional seg.setOptimizeCoefficients (true); // Mandatory seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); //seg.setMaxIterations (1000); seg.setDistanceThreshold (0.01); // Create the filtering object pcl::ExtractIndices<pcl::PointXYZRGB> extract; int i = 0, nr_points = (int) cloud_filtered2->points.size (); // While 50% of the original cloud is still there while (cloud_filtered2->points.size () > 0.5 * nr_points) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered2); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cerr << "Could not estimate a planar model for the given dataset." << std::endl; break; } if( (fabs(coefficients->values[0]) < 0.02) && (fabs(coefficients->values[1]) < 0.02) && (fabs(coefficients->values[2]) > 0.9) ) { std::cerr << "Model coefficients: " << coefficients->values[0] << " " << coefficients->values[1] << " " << coefficients->values[2] << " " << coefficients->values[3] << std::endl; height = coefficients->values[3]; // Extract the inliers extract.setInputCloud (cloud_filtered2); extract.setIndices (inliers); extract.setNegative (false); extract.filter (*cloud_filtered3); // Transformation into ROS type //pcl::toPCLPointCloud2(*(cloud_filtered3), *(cloud_out)); //pcl_conversions::moveFromPCL(*(cloud_out), pc2); //debug_pub.publish(pc2); } // Create the filtering object extract.setNegative (true); extract.filter (*cloud_f); cloud_filtered2.swap (cloud_f); i++; } /* pass.setInputCloud (cloud_filtered2); pass.setFilterFieldName ("z"); pass.setFilterLimits (height, 1.5); //pass.setFilterLimitsNegative (true); pass.filter (*cloud_filtered2); */ // Transformation into ROS type //pcl::toPCLPointCloud2(*(cloud_filtered2), *(cloud_out)); //pcl_conversions::moveFromPCL(*(cloud_out), pc2); //debug_pub.publish(pc2); ///////////////////////////////// // Statistical Outlier Removal // ///////////////////////////////// pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor; sor.setInputCloud (cloud_filtered2); sor.setMeanK (50); sor.setStddevMulThresh (1.0); sor.filter (*cloud_filtered2); ////////////////////////////////// // Euclidian Cluster Extraction // ////////////////////////////////// // Creating the KdTree object for the search method of the extraction pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>); tree->setInputCloud (cloud_filtered2); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec; ec.setClusterTolerance (0.02); // 2cm ec.setMinClusterSize (50); ec.setMaxClusterSize (5000); ec.setSearchMethod (tree); ec.setInputCloud (cloud_filtered2); ec.extract (cluster_indices); int j = 0; for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster1 (new pcl::PointCloud<pcl::PointXYZRGB>); for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++) cloud_cluster1->points.push_back (cloud_filtered2->points[*pit]); cloud_cluster1->width = cloud_cluster1->points.size (); cloud_cluster1->height = 1; cloud_cluster1->is_dense = true; cloud_cluster1->header.frame_id = "/map"; if(j == 0) { // Transformation into ROS type pcl::toPCLPointCloud2(*(cloud_cluster1), *(cloud_out)); pcl_conversions::moveFromPCL(*(cloud_out), pc2); debug_pub.publish(pc2); } else { // Transformation into ROS type pcl::toPCLPointCloud2(*(cloud_cluster1), *(cloud_out)); pcl_conversions::moveFromPCL(*(cloud_out), pc2); debug2_pub.publish(pc2); } j++; } // Transformation into ROS type //pcl::toPCLPointCloud2(*(cloud_filtered2), *(cloud_out)); //pcl_conversions::moveFromPCL(*(cloud_out), pc2); //debug2_pub.publish(pc2); //debug2_pub.publish(*pc_map); } }