Eigen::Vector3d estimate_plane_normals(PointCloud::Ptr cloud_f) { PointCloud::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>),cloud_plane (new pcl::PointCloud<pcl::PointXYZRGB>),cloud_f_aux (new pcl::PointCloud<pcl::PointXYZRGB>); PointCloud::Ptr cloud_filtered_while (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PassThrough<pcl::PointXYZRGB> pass; pass.setInputCloud (cloud_f); pass.setFilterFieldName ("z"); pass.setFilterLimits (0.0, 1.1); //pass.setFilterLimitsNegative (true); pass.filter (*cloud_filtered); pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor; sor.setInputCloud (cloud_filtered); sor.setMeanK (50); sor.setStddevMulThresh (2); sor.filter (*cloud_filtered); 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.setDistanceThreshold (0.01); cloud_filtered_while =cloud_filtered; int i = 0, nr_points = (int) cloud_filtered_while->points.size (); // While 30% of the original cloud is still there pcl::ExtractIndices<pcl::PointXYZRGB> extract; while (cloud_filtered_while->points.size () > 0.3 * nr_points) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered_while); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cerr << "Could not estimate a planar model for the given dataset." << std::endl; break; } // Extract the planar inliers from the input cloud extract.setInputCloud (cloud_filtered_while); extract.setIndices (inliers); // Remove the planar inliers, extract the rest extract.setNegative (false); extract.filter (*cloud_plane); // Create the filtering object extract.setNegative (true); extract.filter (*cloud_f_aux); cloud_filtered_while.swap (cloud_f_aux); i++; } Eigen::Vector3d plane_normal_vector ; for(int i=0;i<3;i++) plane_normal_vector(i) = coefficients->values[i]; return plane_normal_vector; }
void cloud_cb (sensor_msgs::PointCloud2ConstPtr input) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::fromROSMsg (*input, *cloud_filtered); //std::cout << "Input pointCloud has: " << cloud_filtered->points.size () << " data points." << std::endl; //* // Create the segmentation object for the planar model and set all the parameters pcl::SACSegmentation<pcl::PointXYZRGB> seg; pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZRGB> ()); seg.setOptimizeCoefficients (true); seg.setModelType (ModelType);//(pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (MaxIterations);//(100); seg.setDistanceThreshold (DistanceThreshold);//(0.02); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>); int i=0, nr_points = (int) cloud_filtered->points.size (); while (cloud_filtered->points.size () > RatioLimit * nr_points)//0.3 * nr_points) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cout << "Could not estimate a planar model for the given dataset." << std::endl; break; } // Extract the planar inliers from the input cloud pcl::ExtractIndices<pcl::PointXYZRGB> extract; extract.setInputCloud (cloud_filtered); extract.setIndices (inliers); // extract.setNegative (false); // // Write the planar inliers to disk // extract.filter (*cloud_plane); // std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl; // Remove the planar inliers, extract the rest extract.setNegative (ExtractNegative); extract.filter (*cloud_f); cloud_filtered = cloud_f; } sensor_msgs::PointCloud2 output; pcl::toROSMsg (*cloud_filtered , output); output.header.stamp = ros::Time::now(); output.header.frame_id = "/camera_rgb_optical_frame"; // Publish the data cloud_pub.publish (output); }
void Planar_filter::Segment_cloud() { pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); PointCloudT::Ptr cloud_plane (new PointCloudT ()); seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); seg.setDistanceThreshold (0.02); std::vector<pcl::PointIndices> cluster_indices; seg.setInputCloud (_cloud); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cout << "Could not estimate a planar model for the given dataset." << std::endl; } // Extract the planar inliers from the input cloud pcl::ExtractIndices<PointT> extract; extract.setInputCloud (_cloud); extract.setIndices (inliers); extract.setNegative (false); // Get the points associated with the planar surface PointCloudT::Ptr cloud_f(new PointCloudT); extract.filter (*cloud_plane); //std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl; extract.setNegative (true); extract.filter (*cloud_f); *_cloud = *cloud_f; // Creating the KdTree object for the search method of the extraction pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>); tree->setInputCloud (_cloud); pcl::EuclideanClusterExtraction<PointT> ec; ec.setClusterTolerance (0.02); // 2cm ec.setMinClusterSize (100); ec.setMaxClusterSize (25000); ec.setSearchMethod (tree); ec.setInputCloud (_cloud); ec.extract (cluster_indices); for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) { PointCloudT::Ptr cloud_cluster (new PointCloudT); for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit) cloud_cluster->points.push_back (_cloud->points[*pit]); //* cloud_cluster->width = cloud_cluster->points.size(); cloud_cluster->height = 1; cloud_cluster->is_dense = true; _Segmented_clouds.push_back(cloud_cluster); } }
pcl::PointCloud<pcl::PointXYZ>::Ptr remove_ground_plane(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) { //see also pcl cluster extraction tutorial pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>); // Create the segmentation object for the planar model and set all the parameters pcl::SACSegmentation<pcl::PointXYZ> seg; pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ()); pcl::PCDWriter writer; seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); seg.setDistanceThreshold (0.05); int i=0, nr_points = (int) cloud->points.size (); while (cloud->points.size () > 0.3 * nr_points) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cout << "Could not estimate a planar model for the given dataset." << std::endl; break; } // Extract the planar inliers from the input cloud pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud (cloud); extract.setIndices (inliers); extract.setNegative (false); // Get the points associated with the planar surface extract.filter (*cloud_plane); std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl; // Remove the planar inliers, extract the rest extract.setNegative (true); extract.filter (*cloud_f); *cloud = *cloud_f; } return cloud; }
PointCloud<PointXYZ>::Ptr PCLTools::segmentPlanar(PointCloud<PointXYZ>::Ptr cloud, bool negative) { // Create the segmentation object for the planar model and set all the parameters PointCloud<PointXYZ>::Ptr cloud_f(new PointCloud<PointXYZ>); SACSegmentation<PointXYZ> seg; PointIndices::Ptr inliers(new PointIndices); ModelCoefficients::Ptr coefficients(new ModelCoefficients); PointCloud<PointXYZ>::Ptr cloud_plane(new PointCloud<PointXYZ> ()); seg.setOptimizeCoefficients(true); seg.setModelType(pcl::SACMODEL_PLANE); seg.setMethodType(pcl::SAC_RANSAC); seg.setMaxIterations(100); seg.setDistanceThreshold(0.02); // Segment the largest planar component from the remaining cloud seg.setInputCloud(cloud); seg.segment(*inliers, *coefficients); if (inliers->indices.size () == 0) { cerr << "Could not estimate a planar model for the given dataset." << endl; return cloud_f; } // Extract the planar inliers from the input cloud ExtractIndices<PointXYZ> extract; extract.setInputCloud(cloud); extract.setIndices(inliers); extract.setNegative(false); // Get the points associated with the planar surface extract.filter(*cloud_plane); // Remove the planar inliers, extract the rest extract.setNegative(negative); extract.filter(*cloud_f); return cloud_f; }
/* ======================================== * Fill Code: PLANE SEGEMENTATION * ========================================*/ pcl::PointCloud<pcl::PointXYZ>::Ptr planeSegmentation(const pcl::PointCloud<pcl::PointXYZ>::Ptr (&input_cloud)) { pcl::PointCloud<pcl::PointXYZ>::Ptr cropped_cloud (new pcl::PointCloud<pcl::PointXYZ>(*input_cloud)); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ()); // Create the segmentation object for the planar model and set all the parameters pcl::SACSegmentation<pcl::PointXYZ> seg; pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (200); seg.setDistanceThreshold (0.01); //keep as 1cm // Segment the largest planar component from the cropped cloud seg.setInputCloud (cropped_cloud); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { ROS_WARN_STREAM ("Could not estimate a planar model for the given dataset.") ; //break; } // Extract the planar inliers from the input cloud pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud (cropped_cloud); extract.setIndices(inliers); extract.setNegative (false); // Get the points associated with the planar surface extract.filter (*cloud_plane); ROS_INFO_STREAM("PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." ); // Remove the planar inliers, extract the rest extract.setNegative (true); extract.filter (*cloud_f); return cloud_f; }
int main (int argc, char** argv) { // All the objects needed pcl::PCDReader reader; pcl::PassThrough<PointT> pass; pcl::NormalEstimation<PointT, pcl::Normal> ne; pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg; pcl::PCDWriter writer; pcl::ExtractIndices<PointT> extract; pcl::ExtractIndices<pcl::Normal> extract_normals; pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ()); // Datasets pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>); pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>); pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>); pcl::PointCloud<PointT>::Ptr cloud_filtered2 (new pcl::PointCloud<PointT>); pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>); pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients), coefficients_cylinder (new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices), inliers_cylinder (new pcl::PointIndices); // Read in the cloud data reader.read ("table_scene_mug_stereo_textured.pcd", *cloud); std::cerr << "PointCloud has: " << cloud->points.size () << " data points." << std::endl; // Build a passthrough filter to remove spurious NaNs pass.setInputCloud (cloud); pass.setFilterFieldName ("z"); pass.setFilterLimits (0, 1.5); pass.filter (*cloud_filtered); std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; // Estimate point normals ne.setSearchMethod (tree); ne.setInputCloud (cloud_filtered); ne.setKSearch (50); ne.compute (*cloud_normals); // Create the segmentation object for the planar model and set all the parameters seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_NORMAL_PLANE); seg.setNormalDistanceWeight (0.1); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); seg.setDistanceThreshold (0.03); seg.setInputCloud (cloud_filtered); seg.setInputNormals (cloud_normals); // Obtain the plane inliers and coefficients seg.segment (*inliers_plane, *coefficients_plane); std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl; // Extract the planar inliers from the input cloud extract.setInputCloud (cloud_filtered); extract.setIndices (inliers_plane); extract.setNegative (false); // Write the planar inliers to disk pcl::PointCloud<PointT>::Ptr cloud_plane (new pcl::PointCloud<PointT> ()); extract.filter (*cloud_plane); std::cerr << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl; writer.write ("table_scene_mug_stereo_textured_plane.pcd", *cloud_plane, false); // Remove the planar inliers, extract the rest extract.setNegative (true); extract.filter (*cloud_filtered2); extract_normals.setNegative (true); extract_normals.setInputCloud (cloud_normals); extract_normals.setIndices (inliers_plane); extract_normals.filter (*cloud_normals2); // Create the segmentation object for cylinder segmentation and set all the parameters seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_CYLINDER); seg.setMethodType (pcl::SAC_RANSAC); seg.setNormalDistanceWeight (0.1); seg.setMaxIterations (10000); seg.setDistanceThreshold (0.05); seg.setRadiusLimits (0, 0.1); seg.setInputCloud (cloud_filtered2); seg.setInputNormals (cloud_normals2); // Obtain the cylinder inliers and coefficients seg.segment (*inliers_cylinder, *coefficients_cylinder); std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl; // Write the cylinder inliers to disk extract.setInputCloud (cloud_filtered2); extract.setIndices (inliers_cylinder); extract.setNegative (false); pcl::PointCloud<PointT>::Ptr cloud_cylinder (new pcl::PointCloud<PointT> ()); extract.filter (*cloud_cylinder); if (cloud_cylinder->points.empty ()) std::cerr << "Can't find the cylindrical component." << std::endl; else { std::cerr << "PointCloud representing the cylindrical component: " << cloud_cylinder->points.size () << " data points." << std::endl; writer.write ("table_scene_mug_stereo_textured_cylinder.pcd", *cloud_cylinder, false); } return (0); }
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input) { // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg (*input, *cloud); pcl::VoxelGrid<pcl::PointXYZ> vg; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>); vg.setInputCloud(cloud); vg.setLeafSize(0.01f, 0.01f, 0.01f); vg.filter(*cloud_filtered); pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::SACSegmentation<pcl::PointXYZ> seg; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ>() ); seg.setOptimizeCoefficients(true); seg.setModelType(pcl::SACMODEL_PLANE); seg.setMethodType(pcl::SAC_RANSAC); seg.setMaxIterations(100); seg.setDistanceThreshold(0.02); int i = 0, nr_points = (int) cloud_filtered -> points.size(); while(cloud_filtered -> points.size() > 0.3 * nr_points) { seg.setInputCloud(cloud_filtered); seg.segment(*inliers, *coefficients); if(inliers -> indices.size() == 0) break; pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud(cloud_filtered); extract.setIndices(inliers); extract.setNegative(false); extract.filter(*cloud_plane); extract.setNegative(true); extract.filter(*cloud_f); *cloud_filtered = *cloud_f; } pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>); tree -> setInputCloud(cloud_filtered); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; ec.setClusterTolerance(0.02); ec.setMinClusterSize(100); ec.setMaxClusterSize(25000); ec.setSearchMethod(tree); ec.setInputCloud(cloud_filtered); ec.extract(cluster_indices); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>); for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) { for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit) cloud_cluster->points.push_back (cloud_filtered->points[*pit]); } sensor_msgs::PointCloud2 output; pcl::toROSMsg(*cloud_cluster, output); pub.publish (output); }
int main (int argc, char** argv) { pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBA>); pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGBA>); if (pcl::io::loadPCDFile<pcl::PointXYZRGBA> ("cloud_inliers.pcd", *cloud) == -1) { PCL_ERROR ("Failed to read file cloud_inliers.pcd \n"); return (-1); } std::cout << "Loaded " << cloud->width * cloud->height << " data points from cloud_inliers.pcd." << std::endl; pcl::SACSegmentation<pcl::PointXYZRGBA> seg; pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZRGBA> ()); pcl::PCDWriter writer; seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); seg.setDistanceThreshold (0.02); int i = 0; int num_points = (int) cloud->points.size(); while (cloud->points.size() > 0.3 * num_points) { seg.setInputCloud (cloud); seg.segment (*inliers, *coefficients); if (inliers->indices.size() == 0) { std::cout << "Could not estimate a planar model for the given dataset." << std::endl; break; } pcl::ExtractIndices<pcl::PointXYZRGBA> extract; extract.setInputCloud (cloud); extract.setIndices (inliers); extract.setNegative (false); extract.filter (*cloud_plane); std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl; extract.setNegative (true); extract.filter (*cloud_filtered); *cloud = *cloud_filtered; } pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBA>); tree->setInputCloud (cloud); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZRGBA> ec; ec.setClusterTolerance (0.02); ec.setMinClusterSize (100); ec.setMaxClusterSize (25000); ec.setSearchMethod (tree); ec.setInputCloud (cloud); 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::PointXYZRGBA>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGBA>); for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++) cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //* cloud_cluster->width = cloud_cluster->points.size (); cloud_cluster->height = 1; cloud_cluster->is_dense = true; std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl; std::stringstream ss; ss << "cloud_cluster_" << j << ".pcd"; writer.write<pcl::PointXYZRGBA> (ss.str (), *cloud_cluster, false); //* j++; } return (0); }
bool compute_object(const int i) { // Call get_plane, get_cylinder, get_sphere float plane_score = compute_plane(); // plane_inliers_ has the pc. float cylinder_score = compute_cylinder(); // cylinder_inliers_ has the pc // Select the lowest score and store it in object_inliers_. inliers_object_ = (plane_score > cylinder_score ? inliers_plane_ : inliers_cylinder_); // inliers_object_ = inliers_plane_; // inliers_object_ = inliers_cylinder_; // Test if the point cloud is too small if (inliers_object_->indices.size() < CUT_THRESHOLD) { std::cerr << "object inliers has "<< inliers_object_->indices.size() << " < " << CUT_THRESHOLD <<" points. Aborting..." << std::endl; return false; } /* // Debugging info pcl::PointCloud<Point>::Ptr best_object_(new pcl::PointCloud<Point>); pcl::copyPointCloud(*cloud_filtered, *inliers_object_, *best_object_); std::stringstream debug_s0; debug_s0 << "no_cluster_object_" << i << ".pcd"; pcl::io::savePCDFile(debug_s0.str(), *best_object_); */ // Euclidean Clustering std::vector<pcl::PointIndices> clusters; cluster_.setInputCloud(cloud_filtered); cluster_.setIndices(inliers_object_); cluster_.extract(clusters); std::cout << "Number of Euclidian clusters found: " << clusters.size() << std::endl; // Check if there is no clusters if (clusters.size() == 0) { std::cerr << "No clusters found. Aborting..." << std::endl; return false; } main_cluster_.reset(new pcl::PointIndices(clusters.at(0))); // Extract the inliers from the input cloud extract.setInputCloud(cloud_filtered); extract.setIndices(main_cluster_); extract.setNegative(false); pcl::PointCloud<Point>::Ptr cloud_plane(new pcl::PointCloud<Point > ()); extract.filter(*cloud_plane); // Test if cluster[0] is too small if (main_cluster_->indices.size() < CUT_THRESHOLD) { std::cerr << "object cluster[0] has "<< main_cluster_->indices.size() << " < " << CUT_THRESHOLD <<" points. Not writing to disk..." << std::endl; } else { // Write the inliers to disk std::stringstream debug_s; if (inliers_object_ == inliers_cylinder_) { debug_s << "cylinder_object_" << i << ".pcd"; } else if (inliers_object_ == inliers_plane_) { debug_s << "plane_object_" << i << ".pcd"; } else { debug_s << "unknown_object_" << i << ".pcd"; } pcl::io::savePCDFile(debug_s.str(), *cloud_plane); std::cout << "PointCloud representing the extracted component: " << cloud_plane->points.size() << " data points." << std::endl; } // Remove cluster[0], update cloud_filtered and cloud_normals extract.setNegative(true); extract.filter(*cloud_filtered); extract_normals.setNegative(true); extract_normals.setInputCloud(cloud_normals); extract_normals.setIndices(main_cluster_); extract_normals.filter(*cloud_normals); return true; }
void ClusterExtraction::extract() { /* pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = in_pcl.read(); // Creating the KdTree object for the search method of the extraction pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); tree->setInputCloud (cloud); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; ec.setClusterTolerance (clusterTolerance); // 2cm ec.setMinClusterSize (minClusterSize); ec.setMaxClusterSize (maxClusterSize); ec.setSearchMethod (tree); ec.setInputCloud (cloud); ec.extract (cluster_indices); std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clusters; 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->points[*pit]); //* cloud_cluster->width = cloud_cluster->points.size (); cloud_cluster->height = 1; cloud_cluster->is_dense = true; clusters.push_back(cloud_cluster); std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl; std::stringstream ss; // if(j==0) // { // ss << "cloud_cluster_0" << ".pcd"; // writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); // j++; // } // if(j==0) //{ std::cin.ignore(); out_the_biggest_cluster.write(cloud_cluster); j++; std::cout<<"Zapis!!!\n"; // } } out_indices.write(cluster_indices); out_clusters.write(clusters); //std::cout<<"j=="<<j<<endl; //std::cout<<clusters.size()<<endl; */ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>); // Read in the cloud data pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = in_pcl.read(); std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //* // Create the filtering object: downsample the dataset using a leaf size of 1cm pcl::VoxelGrid<pcl::PointXYZ> vg; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>); vg.setInputCloud (cloud); vg.setLeafSize (0.01f, 0.01f, 0.01f); vg.filter (*cloud_filtered); std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //* // Create the segmentation object for the planar model and set all the parameters pcl::SACSegmentation<pcl::PointXYZ> seg; pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ()); //pcl::PCDWriter writer; seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); seg.setDistanceThreshold (0.02); int i=0, nr_points = (int) cloud_filtered->points.size (); /* while (cloud_filtered->points.size () > 0.3 * nr_points) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cout << "Could not estimate a planar model for the given dataset." << std::endl; break; } // Extract the planar inliers from the input cloud pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud (cloud_filtered); extract.setIndices (inliers); extract.setNegative (false); // Get the points associated with the planar surface extract.filter (*cloud_plane); std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl; // Remove the planar inliers, extract the rest extract.setNegative (true); extract.filter (*cloud_f); *cloud_filtered = *cloud_f; } */ // Creating the KdTree object for the search method of the extraction pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); tree->setInputCloud (cloud_filtered); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; ec.setClusterTolerance (0.02); // 2cm ec.setMinClusterSize (100); ec.setMaxClusterSize (25000); ec.setSearchMethod (tree); ec.setInputCloud (cloud_filtered); 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::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_filtered->points[*pit]); //* cloud_cluster->width = cloud_cluster->points.size (); cloud_cluster->height = 1; cloud_cluster->is_dense = true; std::cout << "PointCloud representing the Cluster: " << 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); //* //std::cin.ignore(); // check if cluster contain interesting us plane, if so, return cluster, next remove plane and we have object :) //if (include_plane){ out_the_biggest_cluster.write(cloud_cluster); break; //} j++; } }
int main (int argc, char** argv) { // Read in the cloud data pcl::PCDReader reader; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>); reader.read ("../bottle.pcd", *cloud); std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //* parseCommandLine(argc, argv); std::cout << "argc:" << argc << " argv:" << *argv << std::endl; std::cout << "x_min:" << x_pass_min_ << " x_max:" << x_pass_max_ << " y_min:" << y_pass_min_ << " y_max:"<<y_pass_max_<<std::endl; /*apply pass through filter*/ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>); // Create the filtering object pcl::PassThrough<pcl::PointXYZ> pass; //pass.setFilterLimitsNegative (true); pass.setInputCloud (cloud); pass.setFilterFieldName ("z"); pass.setFilterLimits (z_pass_min_, z_pass_max_); pass.filter (*cloud_filtered); pass.setInputCloud (cloud_filtered); pass.setFilterFieldName ("y"); pass.setFilterLimits (y_pass_min_, y_pass_max_); pass.filter (*cloud_filtered); pass.setInputCloud (cloud_filtered); pass.setFilterFieldName ("x"); pass.setFilterLimits (x_pass_min_, x_pass_max_); pass.filter (*cloud_filtered); viewer.addPointCloud<PointT> (cloud_filtered, "input_cloud"); #if 0 // while (!viewer.wasStopped ()) { // viewer.spinOnce (); } return(0); #endif viewer.removeAllPointClouds(); cloud = cloud_filtered; #if 0 #if 0 // Create the filtering object: downsample the dataset using a leaf size of 1cm pcl::VoxelGrid<pcl::PointXYZ> vg; //pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>); vg.setInputCloud (cloud); vg.setLeafSize (0.01f, 0.01f, 0.01f); vg.filter (*cloud_filtered); std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //* #endif // Create the segmentation object for the planar model and set all the parameters pcl::SACSegmentation<pcl::PointXYZ> seg; pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ()); pcl::PCDWriter writer; seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); seg.setDistanceThreshold (0.02); int i=0, nr_points = (int) cloud_filtered->points.size (); while (cloud_filtered->points.size () > 0.3 * nr_points) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cout << "Could not estimate a planar model for the given dataset." << std::endl; break; } // Extract the planar inliers from the input cloud pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud (cloud_filtered); extract.setIndices (inliers); extract.setNegative (false); // Get the points associated with the planar surface extract.filter (*cloud_plane); std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl; // Remove the planar inliers, extract the rest extract.setNegative (true); extract.filter (*cloud_f); *cloud_filtered = *cloud_f; } viewer.addPointCloud<PointT> (cloud_filtered, "input_cloud"); while (!viewer.wasStopped ()) { viewer.spinOnce (); } #endif #if 1 pcl::PCDWriter writer; // Creating the KdTree object for the search method of the extraction pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); tree->setInputCloud (cloud_filtered); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; ec.setClusterTolerance (0.02); // 2cm ec.setMinClusterSize (100); ec.setMaxClusterSize (25000); ec.setSearchMethod (tree); ec.setInputCloud (cloud_filtered); 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::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_filtered->points[*pit]); //* cloud_cluster->width = cloud_cluster->points.size (); cloud_cluster->height = 1; cloud_cluster->is_dense = true; cout << "ss:" << j << std::endl; std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl; std::stringstream ss; ss << "cloud_cluster_" << j << ".pcd"; cout << "ss:" << j << std::endl; writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //* j++; } return (0); #endif }
void ObjectDetector::performSegmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ>), cloud_plane_rotated (new pcl::PointCloud<pcl::PointXYZ>); ROS_INFO("PointCloud before planar segmentation: %d data points.", cloud->width * cloud->height); pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers(new pcl::PointIndices); // Create the segmentation object pcl::SACSegmentation<pcl::PointXYZ> seg; // Optional seg.setOptimizeCoefficients (true); // Fit plane seg.setModelType (pcl::SACMODEL_PLANE); // Use RANSAC seg.setMethodType (pcl::SAC_RANSAC); // Set maximum number of iterations seg.setMaxIterations (max_it_calibration); // Set distance to the model threshold seg.setDistanceThreshold (floor_threshold); // Segment the largest planar component from the cloud seg.setInputCloud (cloud); seg.segment (*inliers, *coefficients); // Extract the inliers of the plane pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud (cloud); extract.setIndices (inliers); extract.setNegative (false); extract.filter (*cloud_plane); ROS_INFO("PointCloud representing the planar component: %d data points.", cloud_plane->width * cloud_plane->height); // Create normal vector of the plane Eigen::Matrix<float, 1, 3> normal_plane, normal_floor, r_axis; normal_plane[0] = coefficients->values[0]; normal_plane[1] = coefficients->values[1]; normal_plane[2] = coefficients->values[2]; ROS_INFO("Plane normal: %f %f %f", normal_plane[0], normal_plane[1], normal_plane[2]); // Create normal vector of the floor normal_floor[0] = 0.0f; normal_floor[1] = 1.0f; normal_floor[2] = 0.0f; ROS_INFO("Floor normal: %f %f %f", normal_floor[0], normal_floor[1], normal_floor[2]); // Determine rotation axis r_axis = normal_plane.cross(normal_floor); ROS_INFO("Rotation axis: %f %f %f", r_axis[0], r_axis[1], r_axis[2]); // Determine rotation angle theta theta = acos(normal_plane.dot(normal_floor)); ROS_INFO("Rotation angle theta: %f", theta); // Create rotation matrix Eigen::Affine3f transform = Eigen::Affine3f::Identity(); transform.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX())); // Perform rotation on extracted plane pcl::transformPointCloud(*cloud_plane, *cloud_plane_rotated,transform); // Compute y translation by taking the average y values of the plane points int num_of_points = cloud_plane_rotated->width * cloud_plane_rotated->height; for (size_t i = 0; i < num_of_points; ++i) { y_translation += cloud_plane_rotated->points[i].y; } y_translation = y_translation / num_of_points; }
void callback(const sensor_msgs::PointCloud2ConstPtr& cloud) { ros::Time whole_start = ros::Time::now(); ros::Time declare_types_start = ros::Time::now(); // filter pcl::VoxelGrid<sensor_msgs::PointCloud2> voxel_grid; pcl::PassThrough<sensor_msgs::PointCloud2> pass; pcl::ExtractIndices<pcl::PointXYZ> extract_indices; pcl::ExtractIndices<pcl::Normal> extract_normals; // Normal estimation pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimation; pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> segmentation_from_normals; // Create the segmentation object pcl::SACSegmentation<pcl::PointXYZ> seg; pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ()); pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZ> ()); pcl::search::KdTree<pcl::PointXYZ>::Ptr tree3 (new pcl::search::KdTree<pcl::PointXYZ> ()); // The plane and sphere coefficients pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ()); pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients ()); pcl::ModelCoefficients::Ptr coefficients_cylinder (new pcl::ModelCoefficients ()); pcl::ModelCoefficients::Ptr coefficients_sphere (new pcl::ModelCoefficients ()); // The plane and sphere inliers pcl::PointIndices::Ptr inliers (new pcl::PointIndices ()); pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices ()); pcl::PointIndices::Ptr inliers_cylinder (new pcl::PointIndices ()); pcl::PointIndices::Ptr inliers_sphere (new pcl::PointIndices ()); // The point clouds sensor_msgs::PointCloud2::Ptr voxelgrid_filtered (new sensor_msgs::PointCloud2); sensor_msgs::PointCloud2::Ptr plane_output_cloud (new sensor_msgs::PointCloud2); sensor_msgs::PointCloud2::Ptr rest_output_cloud (new sensor_msgs::PointCloud2); sensor_msgs::PointCloud2::Ptr rest_cloud_filtered (new sensor_msgs::PointCloud2); sensor_msgs::PointCloud2::Ptr cylinder_output_cloud (new sensor_msgs::PointCloud2); sensor_msgs::PointCloud2::Ptr sphere_output_cloud (new sensor_msgs::PointCloud2); // The PointCloud pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr remove_transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_output (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_output (new pcl::PointCloud<pcl::PointXYZ>); // The cloud normals pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ()); // for plane pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal> ()); // for cylinder pcl::PointCloud<pcl::Normal>::Ptr cloud_normals3 (new pcl::PointCloud<pcl::Normal> ()); // for sphere ros::Time declare_types_end = ros::Time::now(); ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * Voxel grid Filtering */ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // // Create VoxelGrid filtering // voxel_grid.setInputCloud (cloud); // voxel_grid.setLeafSize (0.01, 0.01, 0.01); // voxel_grid.filter (*voxelgrid_filtered); // // // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud // pcl::fromROSMsg (*voxelgrid_filtered, *transformed_cloud); ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * Passthrough Filtering */ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // pass through filter // pass.setInputCloud (cloud); // pass.setFilterFieldName ("z"); // pass.setFilterLimits (0, 1.5); // pass.filter (*cloud_filtered); // // // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud // pcl::fromROSMsg (*cloud_filtered, *transformed_cloud); ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * Estimate point normals */ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // ros::Time estimate_start = ros::Time::now(); // // // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud // pcl::fromROSMsg (*cloud, *transformed_cloud); // // // Estimate point normals // normal_estimation.setSearchMethod (tree); // normal_estimation.setInputCloud (transformed_cloud); // normal_estimation.setKSearch (50); // normal_estimation.compute (*cloud_normals); // // ros::Time estimate_end = ros::Time::now(); ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * Create and processing the plane extraction */ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // ros::Time plane_start = ros::Time::now(); // // // Create the segmentation object for the planar model and set all the parameters // segmentation_from_normals.setOptimizeCoefficients (true); // segmentation_from_normals.setModelType (pcl::SACMODEL_NORMAL_PLANE); // segmentation_from_normals.setNormalDistanceWeight (0.1); // segmentation_from_normals.setMethodType (pcl::SAC_RANSAC); // segmentation_from_normals.setMaxIterations (100); // segmentation_from_normals.setDistanceThreshold (0.03); // segmentation_from_normals.setInputCloud (transformed_cloud); // segmentation_from_normals.setInputNormals (cloud_normals); // // // Obtain the plane inliers and coefficients // segmentation_from_normals.segment (*inliers_plane, *coefficients_plane); // //std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl; // // // Extract the planar inliers from the input cloud // extract_indices.setInputCloud (transformed_cloud); // extract_indices.setIndices (inliers_plane); // extract_indices.setNegative (false); // extract_indices.filter (*cloud_plane); // // pcl::toROSMsg (*cloud_plane, *plane_output_cloud); // plane_pub.publish(plane_output_cloud); // // ros::Time plane_end = ros::Time::now(); ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ros::Time plane_start = ros::Time::now(); pcl::fromROSMsg (*cloud, *transformed_cloud); seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (1000); seg.setDistanceThreshold (0.01); seg.setInputCloud (transformed_cloud); seg.segment (*inliers_plane, *coefficients_plane); extract_indices.setInputCloud(transformed_cloud); extract_indices.setIndices(inliers_plane); extract_indices.setNegative(false); extract_indices.filter(*cloud_plane); pcl::toROSMsg (*cloud_plane, *plane_output_cloud); plane_pub.publish(plane_output_cloud); ros::Time plane_end = ros::Time::now(); ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * Extract rest plane and passthrough filtering */ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ros::Time rest_pass_start = ros::Time::now(); // Create the filtering object // Remove the planar inliers, extract the rest extract_indices.setNegative (true); extract_indices.filter (*remove_transformed_cloud); transformed_cloud.swap (remove_transformed_cloud); // publish result of Removal the planar inliers, extract the rest pcl::toROSMsg (*transformed_cloud, *rest_output_cloud); rest_pub.publish(rest_output_cloud); // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud // pcl::fromROSMsg (*rest_output_cloud, *cylinder_cloud); // pass through filter pass.setInputCloud (rest_output_cloud); pass.setFilterFieldName ("z"); pass.setFilterLimits (0, 2.5); pass.filter (*rest_cloud_filtered); ros::Time rest_pass_end = ros::Time::now(); ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * for cylinder features */ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud pcl::fromROSMsg (*rest_cloud_filtered, *cylinder_cloud); // Estimate point normals normal_estimation.setSearchMethod (tree2); normal_estimation.setInputCloud (cylinder_cloud); normal_estimation.setKSearch (50); normal_estimation.compute (*cloud_normals2); // Create the segmentation object for sphere segmentation and set all the paopennirameters segmentation_from_normals.setOptimizeCoefficients (true); segmentation_from_normals.setModelType (pcl::SACMODEL_CYLINDER); segmentation_from_normals.setMethodType (pcl::SAC_RANSAC); segmentation_from_normals.setNormalDistanceWeight (0.1); segmentation_from_normals.setMaxIterations (10000); segmentation_from_normals.setDistanceThreshold (0.05); segmentation_from_normals.setRadiusLimits (0, 0.5); segmentation_from_normals.setInputCloud (cylinder_cloud); segmentation_from_normals.setInputNormals (cloud_normals2); // Obtain the sphere inliers and coefficients segmentation_from_normals.segment (*inliers_cylinder, *coefficients_cylinder); //std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl; // Publish the sphere cloud extract_indices.setInputCloud (cylinder_cloud); extract_indices.setIndices (inliers_cylinder); extract_indices.setNegative (false); extract_indices.filter (*cylinder_output); if (cylinder_output->points.empty ()) std::cerr << "Can't find the cylindrical component." << std::endl; pcl::toROSMsg (*cylinder_output, *cylinder_output_cloud); cylinder_pub.publish(cylinder_output_cloud); */ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * for sphere features */ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ros::Time sphere_start = ros::Time::now(); // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud pcl::fromROSMsg (*rest_cloud_filtered, *sphere_cloud); // Estimate point normals normal_estimation.setSearchMethod (tree3); normal_estimation.setInputCloud (sphere_cloud); normal_estimation.setKSearch (50); normal_estimation.compute (*cloud_normals3); // Create the segmentation object for sphere segmentation and set all the paopennirameters segmentation_from_normals.setOptimizeCoefficients (true); //segmentation_from_normals.setModelType (pcl::SACMODEL_SPHERE); segmentation_from_normals.setModelType (pcl::SACMODEL_SPHERE); segmentation_from_normals.setMethodType (pcl::SAC_RANSAC); segmentation_from_normals.setNormalDistanceWeight (0.1); segmentation_from_normals.setMaxIterations (10000); segmentation_from_normals.setDistanceThreshold (0.05); segmentation_from_normals.setRadiusLimits (0, 0.2); segmentation_from_normals.setInputCloud (sphere_cloud); segmentation_from_normals.setInputNormals (cloud_normals3); // Obtain the sphere inliers and coefficients segmentation_from_normals.segment (*inliers_sphere, *coefficients_sphere); //std::cerr << "Sphere coefficients: " << *coefficients_sphere << std::endl; // Publish the sphere cloud extract_indices.setInputCloud (sphere_cloud); extract_indices.setIndices (inliers_sphere); extract_indices.setNegative (false); extract_indices.filter (*sphere_output); if (sphere_output->points.empty ()) std::cerr << "Can't find the sphere component." << std::endl; pcl::toROSMsg (*sphere_output, *sphere_output_cloud); sphere_pub.publish(sphere_output_cloud); ros::Time sphere_end = ros::Time::now(); std::cout << "cloud size : " << cloud->width * cloud->height << std::endl; std::cout << "plane size : " << transformed_cloud->width * transformed_cloud->height << std::endl; //std::cout << "plane size : " << cloud_normals->width * cloud_normals->height << std::endl; //std::cout << "cylinder size : " << cloud_normals2->width * cloud_normals2->height << std::endl; std::cout << "sphere size : " << cloud_normals3->width * cloud_normals3->height << std::endl; ros::Time whole_now = ros::Time::now(); printf("\n"); std::cout << "whole time : " << whole_now - whole_start << " sec" << std::endl; std::cout << "declare types time : " << declare_types_end - declare_types_start << " sec" << std::endl; //std::cout << "estimate time : " << estimate_end - estimate_start << " sec" << std::endl; std::cout << "plane time : " << plane_end - plane_start << " sec" << std::endl; std::cout << "rest and pass time : " << rest_pass_end - rest_pass_start << " sec" << std::endl; std::cout << "sphere time : " << sphere_end - sphere_start << " sec" << std::endl; printf("\n"); }
bool compute_object(const int i, std::vector<pcl::PointIndices > &segments) { // Call get_plane, get_cylinder, get_sphere // float plane_score = compute_plane(); // plane_inliers_ has the pc. // float cylinder_score = compute_cylinder(); // cylinder_inliers_ has the pc // Select the lowest score and store it in object_inliers_. // inliers_object_ = (plane_score > cylinder_score ? inliers_plane_ : inliers_cylinder_); inliers_object_ = inliers_plane_; // inliers_object_ = inliers_cylinder_; // Test if the point cloud is too small if (inliers_object_->indices.size() < CUT_THRESHOLD) { std::cerr << "object inliers has " << inliers_object_->indices.size() << " < " << CUT_THRESHOLD << " points. Aborting..." << std::endl; return false; } /* // Debugging info pcl::PointCloud<Point>::Ptr best_object_(new pcl::PointCloud<Point>); pcl::copyPointCloud(*cloud_filtered, *inliers_object_, *best_object_); std::stringstream debug_s0; debug_s0 << "no_cluster_object_" << i << ".pcd"; pcl::io::savePCDFile(debug_s0.str(), *best_object_); */ // Euclidean Clustering std::vector<pcl::PointIndices> clusters; cluster_.setInputCloud(cloud_filtered); cluster_.setIndices(inliers_object_); cluster_.extract(clusters); std::cout << "Number of Euclidian clusters found: " << clusters.size() << std::endl; // Check if there is no clusters if (clusters.size() == 0) { std::cerr << "No clusters found. Aborting..." << std::endl; return false; } main_cluster_.reset(new pcl::PointIndices(clusters.at(0))); // Extract the inliers from the input cloud extract.setInputCloud(cloud_filtered); extract.setIndices(main_cluster_); extract.setNegative(false); pcl::PointCloud<Point>::Ptr cloud_plane(new pcl::PointCloud<Point > ()); extract.filter(*cloud_plane); // Test if cluster[0] is too small if (main_cluster_->indices.size() < CUT_THRESHOLD) { std::cerr << "object cluster[0] has " << main_cluster_->indices.size() << " < " << CUT_THRESHOLD << " points. Not writing to disk..." << std::endl; } else { // Write the inliers to disk std::stringstream debug_s; if (inliers_object_ == inliers_cylinder_) { debug_s << "cylinder_object_" << i << ".pcd"; } else if (inliers_object_ == inliers_plane_) { debug_s << "plane_object_" << i << ".pcd"; } else { debug_s << "unknown_object_" << i << ".pcd"; } pcl::io::savePCDFile(debug_s.str(), *cloud_plane); std::cout << "PointCloud representing the extracted component: " << cloud_plane->points.size() << " data points." << std::endl; // Insert the points to segments for cfg3D pcl::PointIndices::Ptr originalIndices(new pcl::PointIndices()); originalIndices->indices.resize(cloud_plane->size()); for (unsigned int j = 0; j < originalIndices->indices.size(); j++) { originalIndices->indices.at(j) = cloud_plane->points.at(j).rgba; } segments.push_back(*originalIndices); // Debugging for cfg3D pcl::PointCloud<Point>::Ptr cfg_segment_cloud(new pcl::PointCloud<Point > ()); pcl::ExtractIndices<Point> cfg_extract; cfg_extract.setNegative(false); cfg_extract.setInputCloud(cloud); cfg_extract.setIndices(originalIndices); cfg_extract.filter(*cfg_segment_cloud); /* cfg_segment_cloud->points.resize(originalIndices->indices.size()); for (unsigned int j = 0; j < originalIndices->indices.size(); j++) { unsigned int index = originalIndices->indices.at(j); cfg_segment_cloud->points.at(j).x = cloud->points.at(index).x; cfg_segment_cloud->points.at(j).y = cloud->points.at(index).y; cfg_segment_cloud->points.at(j).z = cloud->points.at(index).z; } */ std::stringstream cfg_debug_s; cfg_debug_s << "cfg_debug_segment_" << i << ".pcd"; pcl::io::savePCDFile(cfg_debug_s.str(), *cfg_segment_cloud); } // Remove cluster[0], update cloud_filtered and cloud_normals extract.setNegative(true); extract.filter(*cloud_filtered); extract_normals.setNegative(true); extract_normals.setInputCloud(cloud_normals); extract_normals.setIndices(main_cluster_); extract_normals.filter(*cloud_normals); return true; }
void ClusterExtractor::computeClusters() { pcl::PointCloud<pcl::PointXYZ>::Ptr scenePCLPointCloudXYZ(new pcl::PointCloud<pcl::PointXYZ>()); pcl::fromPCLPointCloud2(*sceneCloud, *scenePCLPointCloudXYZ); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>); // Create the filtering object: downsample the dataset using a leaf size of 1cm pcl::VoxelGrid<pcl::PointXYZ> vg; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>); vg.setInputCloud (scenePCLPointCloudXYZ); vg.setLeafSize (0.01f, 0.01f, 0.01f); vg.filter (*cloud_filtered); std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //* // Create the segmentation object for the planar model and set all the parameters pcl::SACSegmentation<pcl::PointXYZ> seg; pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); seg.setDistanceThreshold (0.02); int i=0, nr_points = (int) cloud_filtered->points.size (); while (true) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ()); // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { ROS_WARN("Could not estimate a planar model for the given dataset."); //std::cout << "Could not estimate a planar model for the given dataset." << std::endl; break; } // Extract the planar inliers from the input cloud pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud (cloud_filtered); extract.setIndices (inliers); extract.setNegative (false); // Get the points associated with the planar surface extract.filter (*cloud_plane); ROS_INFO("%d",++i); std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl; if (cloud_plane->points.size() < .2*nr_points) { break; } // Remove the planar inliers, extract the rest extract.setNegative (true); extract.filter (*cloud_f); *cloud_filtered = *cloud_f; // Add euclidean plane to computer clusters cloudClusters.push_back(cloud_plane); } // Creating the KdTree object for the search method of the extraction pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); tree->setInputCloud (cloud_filtered); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; ec.setClusterTolerance (0.02); // 2cm ec.setMinClusterSize (100); ec.setMaxClusterSize (25000000); ec.setSearchMethod (tree); ec.setInputCloud (cloud_filtered); ec.extract (cluster_indices); 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_filtered->points[*pit]); //* cloud_cluster->width = cloud_cluster->points.size (); cloud_cluster->height = 1; cloud_cluster->is_dense = true; cloudClusters.push_back(cloud_cluster); } }
void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input){ sensor_msgs::PointCloud2::Ptr clusters (new sensor_msgs::PointCloud2); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg(*input, *cloud); pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>); std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; // Create the filtering object: downsample the dataset using a leaf size of 1cm pcl::VoxelGrid<pcl::PointXYZ> vg; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>); vg.setInputCloud (cloud); vg.setLeafSize (0.01f, 0.01f, 0.01f); vg.filter (*cloud_filtered); std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; // Create the segmentation object for the planar model and set all the parameters pcl::SACSegmentation<pcl::PointXYZ> seg; pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ()); pcl::PCDWriter writer; seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); seg.setDistanceThreshold (0.02); int i=0, nr_points = (int) cloud_filtered->points.size (); while (cloud_filtered->points.size () > 0.3 * nr_points) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cout << "Could not estimate a planar model for the given dataset." << std::endl; break; } // Extract the planar inliers from the input cloud pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud (cloud_filtered); extract.setIndices (inliers); extract.setNegative (false); // Get the points associated with the planar surface extract.filter (*cloud_plane); std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl; // Remove the planar inliers, extract the rest extract.setNegative (true); extract.filter (*cloud_f); *cloud_filtered = *cloud_f; } // Creating the KdTree object for the search method of the extraction pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); tree->setInputCloud (cloud_filtered); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; ec.setClusterTolerance (0.02); // 2cm ec.setMinClusterSize (10); ec.setMaxClusterSize (2500); ec.setSearchMethod (tree); ec.setInputCloud (cloud_filtered); ec.extract (cluster_indices); std::vector<pcl::PointIndices>::const_iterator it; std::vector<int>::const_iterator pit; int j = 0; for(it = cluster_indices.begin(); it != cluster_indices.end(); ++it) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>); for(pit = it->indices.begin(); pit != it->indices.end(); pit++) { //push_back: add a point to the end of the existing vector cloud_cluster->points.push_back(cloud_filtered->points[*pit]); /*cloud_cluster->width = cloud_cluster->points.size (); cloud_cluster->height = 1; cloud_cluster->is_dense = true; std::stringstream ss; ss << "cloud_cluster_" << j << ".pcd"; writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //* j++;*/ } //Merge current clusters to whole point cloud *clustered_cloud += *cloud_cluster; } pcl::toROSMsg (*clustered_cloud , *clusters); clusters->header.frame_id = "/camera_depth_frame"; clusters->header.stamp=ros::Time::now(); pub.publish (*clusters); //sensor_msgs::PointCloud2::Ptr output (new sensor_msgs::PointCloud2); //pcl::toROSMsg(*cloud_filtered, *output); //pub_plane.publish(*output); }
// Proccess the point clouds void processPointCloud( const sensor_msgs::PointCloud2ConstPtr& pointcloud_msg ) { ROS_INFO_NAMED("perception","\n\n\n"); ROS_INFO_STREAM_NAMED("perception","Processing new point cloud"); // --------------------------------------------------------------------------------------------- // Start making result result_.blocks.poses.clear(); // Clear last block perception result result_.blocks.header.stamp = pointcloud_msg->header.stamp; result_.blocks.header.frame_id = base_link; // Basic point cloud conversions --------------------------------------------------------------- // Convert from ROS to PCL pcl::PointCloud<pcl::PointXYZRGB> cloud; pcl::fromROSMsg(*pointcloud_msg, cloud); // Make new point cloud that is in our working frame pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZRGB>); // Transform to whatever frame we're working in, probably the arm's base frame, ie "base_link" ROS_INFO_STREAM_NAMED("perception","Waiting for transform..."); ros::spinOnce(); tf_listener_.waitForTransform(base_link, cloud.header.frame_id, cloud.header.stamp, ros::Duration(2.0)); if(!pcl_ros::transformPointCloud(base_link, cloud, *cloud_transformed, tf_listener_)) { if( process_count_ > 1 ) // the first time we can ignore it ROS_ERROR_STREAM_NAMED("perception","Error converting to desired frame"); // Do this to speed up the next process attempt: process_count_ = PROCESS_EVERY_NTH; return; } // Limit to things we think are roughly at the table height ------------------------------------ // pcl::PointIndices::Ptr filtered_indices(new pcl::PointIndices); // hold things at table height // std::vector<int> boost::shared_ptr<std::vector<int> > filtered_indices(new std::vector<int>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PassThrough<pcl::PointXYZRGB> pass; pass.setInputCloud(cloud_transformed); pass.setFilterFieldName("z"); pass.setFilterLimits(table_height - 0.05, table_height + block_size + 0.05); //pass.setFilterLimits(table_height - 0.01, table_height + block_size + 0.02); // DTC pass.filter(*filtered_indices); /* // Limit to things in front of the robot --------------------------------------------------- pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>); //pass.setInputCloud(cloud_filteredZ); pass.setIndices(filtered_indices); pass.setFilterFieldName("x"); pass.setFilterLimits(.1,.5); pass.filter(*cloud_filtered); */ /* // Check if any points remain if( cloud_filtered->points.size() == 0 ) { ROS_ERROR_STREAM_NAMED("perception","0 points left"); return; } else { ROS_INFO_STREAM_NAMED("perception","Filtered, %d points left", (int) cloud_filtered->points.size()); } */ // Segment components -------------------------------------------------------------------------- // Create the segmentation object for the planar model and set all the parameters pcl::SACSegmentation<pcl::PointXYZRGB> seg; seg.setOptimizeCoefficients(true); seg.setModelType(pcl::SACMODEL_PLANE); seg.setMethodType(pcl::SAC_RANSAC); // robustness estimator - RANSAC is simple seg.setMaxIterations(200); // the maximum number of iterations the sample consensus method will run seg.setDistanceThreshold(0.005); // determines how close a point must be to the model in order to be considered an inlier pcl::PointIndices::Ptr inliers(new pcl::PointIndices); pcl::ModelCoefficients::Ptr model_coefficients(new pcl::ModelCoefficients); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZRGB>()); /* int nr_points = cloud_filtered->points.size(); // Segment cloud until there are less than 30% of points left? not sure why this is necessary while(cloud_filtered->points.size() > 0.3 * nr_points) { // Segment the largest planar component from the remaining cloud (find the table) seg.setInputCloud(cloud_filtered); // seg.setIndices(); seg.segment(*inliers, *model_coefficients); if(inliers->indices.size() == 0) { ROS_ERROR_STREAM_NAMED("perception","Could not estimate a planar model for the given dataset."); return; } //std::cout << "Inliers: " << (inliers->indices.size()) << std::endl; // Extract the planar inliers from the input cloud pcl::ExtractIndices<pcl::PointXYZRGB> extract; extract.setInputCloud(cloud_filtered); extract.setIndices(inliers); extract.setNegative(false); // Copy the extracted component (the table) to a seperate point cloud extract.filter(*cloud_plane); //std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl; // Remove the planar inliers, extract the rest extract.setNegative(true); extract.filter(*cloud_filtered); // remove table from cloud_filtered // Debug output - DTC // Show the contents of the inlier set, together with the estimated plane parameters, in ax+by+cz+d=0 form (general equation of a plane) ROS_INFO_STREAM_NAMED("perception", "Model coefficients: " << model_coefficients->values[0] << " " << model_coefficients->values[1] << " " << model_coefficients->values[2] << " " << model_coefficients->values[3] ); // TODO: turn this into an rviz marker somehow? */ // Show groups of recognized objects (the inliers) /*std::cerr << "Model inliers: " << inliers->indices.size () << std::endl; for (size_t i = 0; i < inliers->indices.size (); ++i) { std::cerr << inliers->indices[i] << " " << cloud.points[inliers->indices[i]].x << " " << cloud.points[inliers->indices[i]].y << " " << cloud.points[inliers->indices[i]].z << std::endl; }*/ // } // DTC: Removed to make compatible with PCL 1.5 // Creating the KdTree object for the search method of the extraction // pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTreeFLANN<pcl::PointXYZRGB>); pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>); // tree->setInputCloud(cloud_filtered); tree->setInputCloud(cloud_transformed, filtered_indices); // Find the clusters (objects) on the table std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> cluster_extract; //cluster_extract.setClusterTolerance(0.005); // 5mm - If you take a very small value, it can happen that an actual object can be seen as multiple clusters. On the other hand, if you set the value too high, it could happen, that multiple objects are seen as one cluster. So our recommendation is to just test and try out which value suits your dataset. cluster_extract.setClusterTolerance(0.02); // 2cm cluster_extract.setMinClusterSize(100); cluster_extract.setMaxClusterSize(25000); // cluster_extract.setSearchMethod(tree); // cluster_extract.setInputCloud(cloud_filtered); cluster_extract.setInputCloud(cloud_transformed); cluster_extract.setIndices(filtered_indices); ROS_INFO_STREAM_NAMED("perception","Extracting..."); cluster_extract.extract(cluster_indices); ROS_INFO_STREAM_NAMED("perception","after cluster extract"); // Publish point cloud data // filtered_pub_.publish(cloud_filtered); // plane_pub_.publish(cloud_plane); ROS_WARN_STREAM_NAMED("perception","Number indicies/clusters: " << cluster_indices.size() ); // processClusters( cluster_indices, pointcloud_msg, cloud_filtered ); processClusters( cluster_indices, cloud_transformed, cloud_filtered, cloud ); // --------------------------------------------------------------------------------------------- // Final results if(result_.blocks.poses.size() > 0) { // Change action state, if we the action is currently active if(action_server_.isActive()) { action_server_.setSucceeded(result_); } // Publish block poses block_pose_pub_.publish(result_.blocks); // Publish rviz markers of the blocks publishBlockLocation(); ROS_INFO_STREAM_NAMED("perception","Finished ---------------------------------------------- "); } else { ROS_INFO_STREAM_NAMED("perception","Couldn't find any blocks this iteration!"); } }
void environement_identification(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered){ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered_inliers (new pcl::PointCloud<pcl::PointXYZRGB> ()); // Create the segmentation object for the planar model and set all the parameters pcl::SACSegmentation<pcl::PointXYZRGB> seg; pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZRGB> ()); seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); seg.setDistanceThreshold (0.02); int i=0, nr_points = (int) cloud_filtered->points.size (); //while (cloud_filtered->points.size () > 0.3 * nr_points) //{ // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cout << "Could not estimate a planar model for the given dataset." << std::endl; //break; }else{ // Extract the planar inliers from the input cloud pcl::ExtractIndices<pcl::PointXYZRGB> extract; extract.setInputCloud (cloud_filtered); extract.setIndices (inliers); extract.setNegative (false); // Get the points associated with the planar surface extract.filter (*cloud_plane); //std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl; // Remove the planar inliers, extract the rest extract.setNegative (true); extract.filter (*cloud_f); *cloud_filtered = *cloud_f; } // } // Create the filtering object pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor; sor.setInputCloud (cloud_filtered); sor.setMeanK (50); sor.setStddevMulThresh (1.0); sor.filter (*cloud_filtered_inliers); std::cout << "cloud filtred after plane extraction : "<<cloud_filtered_inliers->size()<< std::endl; // 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_filtered_inliers); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec; ec.setClusterTolerance (0.09); // 2cm ec.setMinClusterSize (100); ec.setMaxClusterSize (25000); ec.setSearchMethod (tree); ec.setInputCloud (cloud_filtered_inliers); ec.extract (cluster_indices); std::cout << "cluster number : "<<cluster_indices.size()<< std::endl; 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_cluster (new pcl::PointCloud<pcl::PointXYZRGB>); for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit){ j=j%(sizeof(ColorB)/sizeof(*ColorB)); cloud_filtered_inliers->points[*pit].rgb = *reinterpret_cast<float*>(new uint32_t(static_cast<uint32_t>(ColorR[j]) << 16 | static_cast<uint32_t>(ColorG[j]) << 8 | static_cast<uint32_t>(ColorB[j]) )); cloud_cluster->points.push_back (cloud_filtered_inliers->points[*pit]); } cloud_cluster->width = cloud_cluster->points.size (); cloud_cluster->height = 1; cloud_cluster->is_dense = true; std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl; pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud_cluster); (viewer)->addPointCloud<pcl::PointXYZRGB> (cloud_cluster, rgb, "cloud identification"+j); j++; } }
void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &cloud) { if (!viewer.wasStopped()) { // viewer.showCloud (cloud); // writer.write<pcl::PointXYZ> ("kinect_cloud.pcd", *cloud, false); //* // std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; // Create the filtering object: downsample the dataset using a leaf size of 1cm pcl::VoxelGrid<pcl::PointXYZ> vg; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>); vg.setInputCloud (cloud); vg.setLeafSize (0.01f, 0.01f, 0.01f); vg.filter (*cloud_filtered); // std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //* // Create the segmentation object for the planar model and set all the parameters pcl::SACSegmentation<pcl::PointXYZ> seg; pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ()); pcl::PCDWriter writer; seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); seg.setDistanceThreshold (0.02); int i=0, nr_points = (int) cloud_filtered->points.size (); while (cloud_filtered->points.size () > 0.3 * nr_points) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cout << "Could not estimate a planar model for the given dataset." << std::endl; break; } pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>); // Extract the planar inliers from the input cloud pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud (cloud_filtered); extract.setIndices (inliers); extract.setNegative (false); // Write the planar inliers to disk extract.filter (*cloud_plane); // std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl; // Remove the planar inliers, extract the rest extract.setNegative (true); extract.filter (*cloud_f); cloud_filtered = cloud_f; } // Creating the KdTree object for the search method of the extraction pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); tree->setInputCloud (cloud_filtered); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; ec.setClusterTolerance (0.02); // 2cm ec.setMinClusterSize (100); ec.setMaxClusterSize (25000); ec.setSearchMethod (tree); ec.setInputCloud (cloud_filtered); ec.extract (cluster_indices); int j = 0; std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); // 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_filtered->points[*pit]); //* cloud_cluster->width = cloud_cluster->points.size (); cloud_cluster->height = 1; cloud_cluster->is_dense = true; // std::cout << "PointCloud representing the Cluster: " << 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++; viewer.showCloud (cloud_cluster); } } }
int main(int argc, char** argv) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZRGB>); if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("../learn15.pcd", *cloud) == -1) //* load the file { PCL_ERROR("Couldn't read file test_pcd.pcd \n"); return (-1); } 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.setDistanceThreshold(0.01); seg.setInputCloud(cloud); seg.segment(*inliers, *coefficients); if (inliers->indices.size() == 0) { PCL_ERROR("Could not estimate a planar model for the given dataset."); return (-1); } std::cerr << "Model coefficients: " << coefficients->values[0] << " " << coefficients->values[1] << " " << coefficients->values[2] << " " << coefficients->values[3] << std::endl; pcl::ExtractIndices<pcl::PointXYZRGB> extract; extract.setInputCloud(cloud); extract.setIndices(inliers); extract.setNegative(true); //Removes part_of_cloud but retain the original full_cloud //extract.setNegative(true); // Removes part_of_cloud from full cloud and keep the rest extract.filter(*cloud_plane); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor; sor.setInputCloud(cloud_plane); sor.setMeanK(50); sor.setStddevMulThresh(1.0); sor.filter(*cloud_filtered); //cloud.swap(cloud_plane); /* pcl::visualization::CloudViewer viewer("Simple Cloud Viewer"); viewer.showCloud(cloud_plane); while (!viewer.wasStopped()) { } */ Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity(); Eigen::Matrix<float, 1, 3> fitted_plane_norm, xyaxis_plane_norm, rotation_axis; fitted_plane_norm[0] = coefficients->values[0]; fitted_plane_norm[1] = coefficients->values[1]; fitted_plane_norm[2] = coefficients->values[2]; xyaxis_plane_norm[0] = 0.0; xyaxis_plane_norm[1] = 0.0; xyaxis_plane_norm[2] = 1.0; rotation_axis = xyaxis_plane_norm.cross(fitted_plane_norm); float theta = acos(xyaxis_plane_norm.dot(fitted_plane_norm)); //float theta = -atan2(rotation_axis.norm(), xyaxis_plane_norm.dot(fitted_plane_norm)); transform_2.rotate(Eigen::AngleAxisf(theta, rotation_axis)); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_recentered(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::transformPointCloud(*cloud_filtered, *cloud_recentered, transform_2); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>); pcl::copyPointCloud(*cloud_recentered, *cloud_xyz); /////////////////////////////////////////////////////////////////// Eigen::Vector4f pcaCentroid; pcl::compute3DCentroid(*cloud_xyz, pcaCentroid); Eigen::Matrix3f covariance; computeCovarianceMatrixNormalized(*cloud_xyz, pcaCentroid, covariance); Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors); Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors(); std::cout << eigenVectorsPCA.size() << std::endl; if(eigenVectorsPCA.size()!=9) eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1)); Eigen::Matrix4f projectionTransform(Eigen::Matrix4f::Identity()); projectionTransform.block<3, 3>(0, 0) = eigenVectorsPCA.transpose(); projectionTransform.block<3, 1>(0, 3) = -1.f * (projectionTransform.block<3, 3>(0, 0) * pcaCentroid.head<3>()); pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPointsProjected(new pcl::PointCloud<pcl::PointXYZ>); pcl::transformPointCloud(*cloud_xyz, *cloudPointsProjected, projectionTransform); // Get the minimum and maximum points of the transformed cloud. pcl::PointXYZ minPoint, maxPoint; pcl::getMinMax3D(*cloudPointsProjected, minPoint, maxPoint); const Eigen::Vector3f meanDiagonal = 0.5f*(maxPoint.getVector3fMap() + minPoint.getVector3fMap()); boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer; viewer = rgbVis(cloud); // viewer->addPointCloud<pcl::PointXYZ>(cloudPointsProjected, "Simple Cloud"); // viewer->addPointCloud<pcl::PointXYZRGB>(cloud, "Simple Cloud2"); viewer->addCube(minPoint.x, maxPoint.x, minPoint.y, maxPoint.y, minPoint.z , maxPoint.z); while (!viewer->wasStopped()) { viewer->spinOnce(100); boost::this_thread::sleep(boost::posix_time::microseconds(100000)); } /* pcl::PCA< pcl::PointXYZ > pca; pcl::PointCloud< pcl::PointXYZ >::ConstPtr cloud; pcl::PointCloud< pcl::PointXYZ > proj; pca.setInputCloud(cloud); pca.project(*cloud, proj); Eigen::Vector4f proj_min; Eigen::Vector4f proj_max; pcl::getMinMax3D(proj, proj_min, proj_max); pcl::PointXYZ min; pcl::PointXYZ max; pca.reconstruct(proj_min, min); pca.reconstruct(proj_max, max); boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer; viewer->addCube(min.x, max.x, min.y, max.y, min.z, max.z); */ return (0); }
int main (int argc, char** argv) { // Read in the cloud data pcl::PCDReader reader; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>); reader.read ("table_scene_lms400.pcd", *cloud); std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //* // Create the filtering object: downsample the dataset using a leaf size of 1cm pcl::VoxelGrid<pcl::PointXYZ> vg; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>); vg.setInputCloud (cloud); vg.setLeafSize (0.01f, 0.01f, 0.01f); vg.filter (*cloud_filtered); std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //* // Create the segmentation object for the planar model and set all the parameters pcl::SACSegmentation<pcl::PointXYZ> seg; pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ()); pcl::PCDWriter writer; seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); seg.setDistanceThreshold (0.02); int i=0, nr_points = (int) cloud_filtered->points.size (); while (cloud_filtered->points.size () > 0.3 * nr_points) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cout << "Could not estimate a planar model for the given dataset." << std::endl; break; } // Extract the planar inliers from the input cloud pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud (cloud_filtered); extract.setIndices (inliers); extract.setNegative (false); // Get the points associated with the planar surface extract.filter (*cloud_plane); std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl; // Remove the planar inliers, extract the rest extract.setNegative (true); extract.filter (*cloud_f); *cloud_filtered = *cloud_f; } // Creating the KdTree object for the search method of the extraction pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); tree->setInputCloud (cloud_filtered); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; ec.setClusterTolerance (0.02); // 2cm ec.setMinClusterSize (100); ec.setMaxClusterSize (25000); ec.setSearchMethod (tree); ec.setInputCloud (cloud_filtered); 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::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_filtered->points[*pit]); //* cloud_cluster->width = cloud_cluster->points.size (); cloud_cluster->height = 1; cloud_cluster->is_dense = true; // Visualization removed noise printf("\ncloud_cluster\n"); pcl::visualization::PCLVisualizer viewer1 ("cloud_cluster"); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_cluster_handler (cloud_cluster, 0, 0, 0); // Where 255,255,255 are R,G,B colors viewer1.addPointCloud (cloud_cluster, cloud_cluster_handler, "cloud_cluster"); // We add the point cloud to the viewer and pass the color handler //viewer.addCoordinateSystem (1.0, "cloud", 0); viewer1.setBackgroundColor(0.95, 0.95, 0.95, 0); // Setting background to a dark grey viewer1.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud_cluster"); //viewer.setPosition(800, 400); // Setting visualiser window position while (!viewer1.wasStopped ()) { // Display the visualiser untill 'q' key is pressed viewer1.spinOnce (); } std::cout << "PointCloud representing the Cluster: " << 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 argc, char** argv) { // Read in the cloud data pcl::PCDReader reader; pcl::PointCloud<pcl::PointXYZRGB>::Ptr RGBcloud (new pcl::PointCloud<pcl::PointXYZRGB>); reader.read (argv[1], *RGBcloud); std::cout << "RGBPointCloud before filtering has: " << RGBcloud->points.size () << " data points." << std::endl; //* pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudF (new pcl::PointCloud<pcl::PointXYZRGB>); reader.read(argv[1], *cloudF); std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds; clouds.reserve(10); labels.reserve(10); //this is only for the old data sets //for (size_t i=0; i < RGBcloud->points.size();++i) //{ // if (!((RGBcloud->points[i].x > -.35 && RGBcloud->points[i].x < .35) && (RGBcloud->points[i].y > -.35 && RGBcloud->points[i].y < .35))) // { // RGBcloud->points[i].x = 0; // RGBcloud->points[i].y = 0; // RGBcloud->points[i].z = 0; // } //} // Create the filtering object: downsample the dataset using a leaf size of 1cm pcl::VoxelGrid<pcl::PointXYZRGB> vg; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>); vg.setInputCloud (RGBcloud); vg.setLeafSize (0.001f, 0.001f, 0.001f); vg.filter (*cloud_filtered); std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl; //pcl::visualization::CloudViewer viewer("Cloud Viewer"); // Create the segmentation object for the planar model and set all the parameters pcl::SACSegmentation<pcl::PointXYZRGB> seg; pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZRGB> ()); pcl::PCDWriter writer; seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); seg.setDistanceThreshold (0.0085);//lesser the values, more points seep into the table // Segment the largest planar component from the remaining cloud until 30% of the points remain int i=0, nr_points = (int) cloud_filtered->points.size(); while (cloud_filtered->points.size () > 0.30* nr_points) { seg.setInputCloud(cloud_filtered); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cout << "Could not estimate a planar model for the given dataset." << std::endl; exit(-1); } // Extract the planar inliers from the input cloud pcl::ExtractIndices<pcl::PointXYZRGB> extract; extract.setInputCloud (cloud_filtered); extract.setIndices (inliers); extract.setNegative (false); // Write the planar inliers to disk extract.filter (*cloud_plane); //* std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl; //viewer.showCloud(cloud_plane, "cloud_name"); //std::cin.get(); // Remove the planar inliers, extract the rest extract.setNegative (true); extract.filter (*cloud_filtered); //* std::cerr <<" The Coefficients are: " << coefficients->values[0]<< " "<< coefficients->values[1]<< " "<< coefficients->values[2]<< " " << coefficients->values[3]<< " "<< std::endl; } // color segmentation // // define this vector so we can operate over all colored point clouds in a loop std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> coloredClouds(5); // instantiate the colored clouds for (int i = 0; i < 5; i++) coloredClouds[i] = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>); // alias the colored clouds with names pcl::PointCloud<pcl::PointXYZRGB>::Ptr Rcloud = coloredClouds[0]; pcl::PointCloud<pcl::PointXYZRGB>::Ptr Gcloud = coloredClouds[1]; pcl::PointCloud<pcl::PointXYZRGB>::Ptr Bcloud = coloredClouds[2]; pcl::PointCloud<pcl::PointXYZRGB>::Ptr Ycloud = coloredClouds[3]; pcl::PointCloud<pcl::PointXYZRGB>::Ptr Ocloud = coloredClouds[4]; // color segment the cloud using min/max ranges //colorSegment(cloud_filtered, Rcloud, Gcloud, Bcloud, Ycloud, Ocloud); // color segment the cloud using a normal distrbuted color model normalSegment(cloud_filtered, Rcloud, Gcloud, Bcloud, Ycloud, Ocloud); // temp: display the colored clouds one after another pcl::visualization::CloudViewer viewer("Cloud Viewer"); for (int i = 0; i < coloredClouds.size(); i++) { cout << "Showing cloud #" << i << endl; viewer.showCloud(coloredClouds[i]); cin.get(); } // empty out the lists before re-filling them with colorful points clouds.clear(); labels.clear(); // now find clusters inside each of the colored point clouds int num_clusters = 0; for (int i = 0; i < coloredClouds.size(); i++) { if (coloredClouds[i]->size() == 0) continue; cout << "Clustering colored cloud #" << i << " which has " << coloredClouds[i]->size() << " points" << endl; num_clusters += clusterExtraction(coloredClouds[i], &clouds, &labels); } // display what we've found std::cerr<<"Waiting 3 "<<std::endl; viewer.runOnVisualizationThreadOnce(label); char c_name[] = "Cloud No: "; char cloud_name [20]; std::cerr << "Total Clusters: " << num_clusters << std::endl; for (size_t k = 0; k < num_clusters; k++) { std::sprintf(cloud_name,"%s%d",c_name,k); viewer.showCloud(clouds[k], cloud_name); } std::cin.get(); int xx, yy; while(1) { cout << "Enter the label of the first cloud: "; cin >> xx; cout << "Enter the label of the second cloud: "; cin >> yy; cout << "Finding distance..."; cout << findDistance(clouds[xx], clouds[yy]) << endl; viewer.runOnVisualizationThreadOnce(drawArrow); cout << "Enter the number of the cloud you want to see alone: "; cin >> xx; } while (!viewer.wasStopped ()); return 0; }
void ImageProcessing::segEuclideanClusters(const char *filename) { // Read in the cloud data pcl::PCDReader reader; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>), cloud_f(new pcl::PointCloud<pcl::PointXYZ>); reader.read(filename, *cloud); std::cout << "PointCloud before filtering has: " << cloud->points.size() << " data points." << std::endl; //* // Create the filtering object: downsample the dataset using a leaf size of 1cm pcl::VoxelGrid<pcl::PointXYZ> vg; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>); vg.setInputCloud(cloud); vg.setLeafSize(0.01f, 0.01f, 0.01f); vg.filter(*cloud_filtered); std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl; //* // Create the segmentation object for the planar model and set all the parameters pcl::SACSegmentation<pcl::PointXYZ> seg; pcl::PointIndices::Ptr inliers(new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ> ()); pcl::PCDWriter writer; seg.setOptimizeCoefficients(true); seg.setModelType(pcl::SACMODEL_PLANE); seg.setMethodType(pcl::SAC_RANSAC); seg.setMaxIterations(100); seg.setDistanceThreshold(0.02); int i = 0, nr_points = (int) cloud_filtered->points.size(); while (cloud_filtered->points.size() > 0.3 * nr_points) { // Segment the largest planar component from the remaining cloud seg.setInputCloud(cloud_filtered); seg.segment(*inliers, *coefficients); if (inliers->indices.size() == 0) { std::cout << "Could not estimate a planar model for the given dataset." << std::endl; break; } // Extract the planar inliers from the input cloud pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud(cloud_filtered); extract.setIndices(inliers); extract.setNegative(false); // Get the points associated with the planar surface extract.filter(*cloud_plane); std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl; // Remove the planar inliers, extract the rest extract.setNegative(true); extract.filter(*cloud_f); *cloud_filtered = *cloud_f; } // Creating the KdTree object for the search method of the extraction pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>); tree->setInputCloud(cloud_filtered); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; ec.setClusterTolerance(0.02); // 2cm ec.setMinClusterSize(100); ec.setMaxClusterSize(25000); ec.setSearchMethod(tree); ec.setInputCloud(cloud_filtered); ec.extract(cluster_indices); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr all_clusters(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointXYZRGB aPoint; int j = 0; Color myColor; for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it) { myColor = Color::random(); //one color for each cluster. //adding all points of one cluster for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++) { cloud_cluster->points.push_back(cloud_filtered->points[*pit]); //* aPoint.x = cloud_cluster->points.back().x; aPoint.y = cloud_cluster->points.back().y; aPoint.z = cloud_cluster->points.back().z; aPoint.r = myColor.red; aPoint.g = myColor.green; aPoint.b = myColor.blue; all_clusters->points.push_back(aPoint); } cloud_cluster->width = cloud_cluster->points.size(); cloud_cluster->height = 1; cloud_cluster->is_dense = true; std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size() << " data points." << std::endl; std::stringstream ss; ss << "cloud_cluster_" << j << ".pcd"; j++; } //writer.write<pcl::PointXYZRGB> ("clustered_cloud.pcd", *cloud_cluster, false); //* pcl::visualization::CloudViewer viewer("Cluster viewer"); viewer.showCloud(all_clusters); while (!viewer.wasStopped()) { } }
std::vector<GraspHypothesis> Localization::localizeHands(const PointCloud::Ptr& cloud_in, int size_left, const std::vector<int>& indices, bool calculates_antipodal, bool uses_clustering) { double t0 = omp_get_wtime(); std::vector<GraspHypothesis> hand_list; if (size_left == 0 || cloud_in->size() == 0) { //std::cout << "Input cloud is empty!\n"; //std::cout << size_left << std::endl; hand_list.resize(0); return hand_list; } // set camera source for all points (0 = left, 1 = right) //std::cout << "Generating camera sources for " << cloud_in->size() << " points ...\n"; Eigen::VectorXi pts_cam_source(cloud_in->size()); if (size_left == cloud_in->size()) pts_cam_source << Eigen::VectorXi::Zero(size_left); else pts_cam_source << Eigen::VectorXi::Zero(size_left), Eigen::VectorXi::Ones(cloud_in->size() - size_left); // remove NAN points from the cloud std::vector<int> nan_indices; pcl::removeNaNFromPointCloud(*cloud_in, *cloud_in, nan_indices); // reduce point cloud to workspace //std::cout << "Filtering workspace ...\n"; PointCloud::Ptr cloud(new PointCloud); filterWorkspace(cloud_in, pts_cam_source, cloud, pts_cam_source); //std::cout << " " << cloud->size() << " points left\n"; // store complete cloud for later plotting PointCloud::Ptr cloud_plot(new PointCloud); *cloud_plot = *cloud; *cloud_ = *cloud; // voxelize point cloud //std::cout << "Voxelizing point cloud\n"; double t1_voxels = omp_get_wtime(); voxelizeCloud(cloud, pts_cam_source, cloud, pts_cam_source, 0.003); double t2_voxels = omp_get_wtime() - t1_voxels; //std::cout << " Created " << cloud->points.size() << " voxels in " << t2_voxels << " sec\n"; // plot camera source for each point in the cloud if (plots_camera_sources_) plot_.plotCameraSource(pts_cam_source, cloud); if (uses_clustering) { //std::cout << "Finding point cloud clusters ... \n"; // Create the segmentation object for the planar model and set all the parameters pcl::SACSegmentation<pcl::PointXYZ> seg; pcl::PointIndices::Ptr inliers(new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>()); seg.setOptimizeCoefficients(true); seg.setModelType(pcl::SACMODEL_PLANE); seg.setMethodType(pcl::SAC_RANSAC); seg.setMaxIterations(100); seg.setDistanceThreshold(0.01); // Segment the largest planar component from the remaining cloud seg.setInputCloud(cloud); seg.segment(*inliers, *coefficients); if (inliers->indices.size() == 0) { //std::cout << " Could not estimate a planar model for the given dataset." << std::endl; hand_list.resize(0); return hand_list; } //std::cout << " PointCloud representing the planar component: " << inliers->indices.size() // << " data points." << std::endl; // Extract the nonplanar inliers pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud(cloud); extract.setIndices(inliers); extract.setNegative(true); std::vector<int> indices_cluster; extract.filter(indices_cluster); PointCloud::Ptr cloud_cluster(new PointCloud); cloud_cluster->points.resize(indices_cluster.size()); Eigen::VectorXi cluster_cam_source(indices_cluster.size()); for (int i = 0; i < indices_cluster.size(); i++) { cloud_cluster->points[i] = cloud->points[indices_cluster[i]]; cluster_cam_source[i] = pts_cam_source[indices_cluster[i]]; } cloud = cloud_cluster; *cloud_plot = *cloud; //std::cout << " PointCloud representing the non-planar component: " << cloud->points.size() // << " data points." << std::endl; } // draw down-sampled and workspace reduced cloud cloud_plot = cloud; // set plotting within handle search on/off bool plots_hands; if (plotting_mode_ == PCL_PLOTTING) plots_hands = true; else plots_hands = false; // find hand configurations HandSearch hand_search(finger_width_, hand_outer_diameter_, hand_depth_, hand_height_, init_bite_, num_threads_, num_samples_, plots_hands); hand_list = hand_search.findHands(cloud, pts_cam_source, indices, cloud_plot, calculates_antipodal, uses_clustering); // remove hands at boundaries of workspace if (filters_boundaries_) { //std::cout << "Filtering out hands close to workspace boundaries ...\n"; hand_list = filterHands(hand_list); //std::cout << " # hands left: " << hand_list.size() << "\n"; } double t2 = omp_get_wtime(); //std::cout << "Hand localization done in " << t2 - t0 << " sec\n"; if (plotting_mode_ == PCL_PLOTTING) //{ plot_.plotHands(hand_list, cloud_plot, ""); //} /* else if (plotting_mode_ == RVIZ_PLOTTING) { plot_.plotGraspsRviz(hand_list, visuals_frame_); } */ return hand_list; }
void PointCloudProcessor::_doEuclideanClusterExtraction(const QList<pcl::PointCloud<PointType>::Ptr> & clouds) { QList<pcl::PointCloud<PointType>::Ptr> clusteredClouds; foreach (pcl::PointCloud<PointType>::Ptr cloud, clouds) { // Create the filtering object: downsample the dataset using a leaf size of 10cm pcl::PointCloud<PointType>::Ptr cloudFiltered (new pcl::PointCloud<PointType> ()); pcl::VoxelGrid<PointType> vg; vg.setInputCloud (cloud); vg.setLeafSize (0.1f, 0.1f, 0.1f); vg.filter (*cloudFiltered); int nr_points = static_cast<int>(cloudFiltered->points.size()); addStatus(QStringLiteral("PointCloud after filtering has: ") + QString::number(nr_points) + QStringLiteral(" data points.")); // Create the segmentation object for the planar model and set all the parameters pcl::SACSegmentation<PointType> seg; pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointCloud<PointType>::Ptr cloud_plane (new pcl::PointCloud<PointType> ()); seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); seg.setDistanceThreshold (0.02); pcl::PointCloud<PointType>::Ptr cloud_f (new pcl::PointCloud<PointType>); while (cloudFiltered->points.size () > 0.3 * nr_points) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloudFiltered); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { addStatus(QStringLiteral("Could not estimate a planar model for the given dataset.")); break; } // Extract the planar inliers from the input cloud pcl::ExtractIndices<PointType> extract; extract.setInputCloud (cloudFiltered); extract.setIndices (inliers); extract.setNegative (false); // Get the points associated with the planar surface extract.filter (*cloud_plane); addStatus(QStringLiteral("PointCloud representing the planar component: ") + QString::number(cloud_plane->points.size()) + QStringLiteral(" data points.")); // Remove the planar inliers, extract the rest extract.setNegative (true); extract.filter (*cloud_f); *cloudFiltered = *cloud_f; } // Creating the KdTree object for the search method of the extraction pcl::search::KdTree<PointType>::Ptr tree (new pcl::search::KdTree<PointType>); tree->setInputCloud (cloudFiltered); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<PointType> ec; ec.setClusterTolerance (0.2); // 20cm ec.setMinClusterSize (100); ec.setMaxClusterSize (250000); ec.setSearchMethod (tree); ec.setInputCloud (cloudFiltered); ec.extract (cluster_indices); for (auto it = cluster_indices.begin (); it != cluster_indices.end (); ++it) { pcl::PointCloud<PointType>::Ptr cloud_cluster (new pcl::PointCloud<PointType>); for (auto pit = it->indices.begin (); pit != it->indices.end (); pit++) { cloud_cluster->points.push_back (cloudFiltered->points[*pit]); //* } cloud_cluster->width = cloud_cluster->points.size (); cloud_cluster->height = 1; cloud_cluster->is_dense = true; clusteredClouds.push_back(cloud_cluster); } }
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input) { // sensor_msgs::PointCloud2 cloud_filtered; pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled_XYZ (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr output_p (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>); sensor_msgs::PointCloud2::Ptr downsampled (new sensor_msgs::PointCloud2); // sensor_msgs::PointCloud2::Ptr output (new sensor_msgs::PointCloud2); // std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height // << " data points." << std::endl; // Create the filtering object: downsample the dataset using a leaf size of 1cm pcl::VoxelGrid<sensor_msgs::PointCloud2> sor; sor.setInputCloud (input); sor.setLeafSize (0.01f, 0.01f, 0.01f); sor.filter (*downsampled); // std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl; // Change from type sensor_msgs::PointCloud2 to pcl::PointXYZ pcl::fromROSMsg (*downsampled, *downsampled_XYZ); //Create the SACSegmentation object and set the model and method type pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ()); pcl::PointIndices::Ptr inliers (new pcl::PointIndices ()); // Create the segmentation object pcl::SACSegmentation<pcl::PointXYZ> seg; // Optional seg.setOptimizeCoefficients (true); // Mandatory seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC);//For more info: wikipedia.org/wiki/RANSAC seg.setMaxIterations (100); seg.setDistanceThreshold (0.02);//How close a point must be to the model to considered an inlier int i = 0, nr_points = (int) downsampled_XYZ->points.size (); //Contains the plane point cloud pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ()); // While 30% of the original cloud is still there while (downsampled_XYZ->points.size () > 0.3 * nr_points) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (downsampled_XYZ); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cerr << "Could not estimate a planar model for the given dataset." << std::endl; break; } // Extract the planar inliers from the input cloud pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud (downsampled_XYZ); extract.setIndices (inliers); extract.setNegative (false); // Get the points associated with the planar surface extract.filter (*cloud_plane); // std::cerr << "PointCloud representing the planar component: " // << output_p->width * output_p->height << " data points." << std::endl; // std::stringstream ss; // ss << "table_scene_lms400_plane_" << i << ".pcd"; // writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false); // Remove the planar inliers, extract the rest extract.setNegative (true); extract.filter (*cloud_f); downsampled_XYZ.swap(cloud_f); i++; } // Creating the KdTree object for the search method of the extraction pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); tree->setInputCloud (downsampled_XYZ); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; ec.setClusterTolerance (0.02); // 2cm ec.setMinClusterSize (100); ec.setMaxClusterSize (25000); ec.setSearchMethod (tree); ec.setInputCloud (downsampled_XYZ); ec.extract (cluster_indices); ros::NodeHandle nh; //Create a publisher for each cluster for (int i = 0; i < cluster_indices.size(); ++i) { std::string topicName = "/pcl_tut/cluster" + boost::lexical_cast<std::string>(i); ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2> (topicName, 1); pub_vec.push_back(pub); } 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 (downsampled_XYZ->points[*pit]); //* cloud_cluster->width = cloud_cluster->points.size (); cloud_cluster->height = 1; cloud_cluster->is_dense = true; // std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl; //Convert the pointcloud to be used in ROS sensor_msgs::PointCloud2::Ptr output (new sensor_msgs::PointCloud2); pcl::toROSMsg (*cloud_cluster, *output); output->header.frame_id = input->header.frame_id; // Publish the data pub_vec[j].publish (output); ++j; } }
// Align a collection of object templates to a sample point cloud void cloud_cb( const sensor_msgs::PointCloud2ConstPtr& input ) { if( controllerState != 1 ) return; //--- Convert Incoming Cloud --- // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud( new pcl::PointCloud<pcl::PointXYZ> ); pcl::fromROSMsg( *input, *cloud ); // --- Z-Filter And Downsample Cloud --- // // Preprocess the cloud by removing distant points pcl::PassThrough<pcl::PointXYZ> pass_z; pass_z.setInputCloud( cloud ); pass_z.setFilterFieldName( "z" ); pass_z.setFilterLimits( 0, 1.75 ); pass_z.filter( *cloud ); pcl::PassThrough<pcl::PointXYZ> pass_y; pass_y.setInputCloud( cloud ); pass_y.setFilterFieldName("y"); pass_y.setFilterLimits( -0.5, 0.2 ); pass_y.filter( *cloud ); pcl::PassThrough<pcl::PointXYZ> pass_x; pass_x.setInputCloud( cloud ); pass_x.setFilterFieldName("x"); pass_x.setFilterLimits( -0.5, 0.5 ); pass_x.filter( *cloud ); // It is possible to not have any points after z-filtering (ex. if we are looking up). // Just bail if there is nothing left. if( cloud->points.size() == 0 ) return; //visualize( cloud, visualizer_o_Ptr ); // --- Calculate Scene Normals --- // pcl::PointCloud<pcl::Normal>::Ptr pSceneNormals( new pcl::PointCloud<pcl::Normal>() ); pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> normEst; normEst.setKSearch(10); normEst.setInputCloud( cloud ); normEst.compute( *pSceneNormals ); // --- Get Rid Of Floor --- // pcl::PointIndices::Ptr inliers_plane( new pcl::PointIndices ); pcl::ModelCoefficients::Ptr coefficients_plane( new pcl::ModelCoefficients ); pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg1; seg1.setOptimizeCoefficients( true ); seg1.setModelType( pcl::SACMODEL_NORMAL_PLANE ); seg1.setNormalDistanceWeight( 0.05 ); seg1.setMethodType( pcl::SAC_RANSAC ); seg1.setMaxIterations( 100 ); seg1.setDistanceThreshold( 0.075 ); seg1.setInputCloud( cloud ); seg1.setInputNormals( pSceneNormals ); // Obtain the plane inliers and coefficients seg1.segment( *inliers_plane, *coefficients_plane ); // Extract the planar inliers from the input cloud pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud( cloud ); extract.setIndices( inliers_plane ); extract.setNegative( false ); // Write the planar inliers to disk pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane( new pcl::PointCloud<pcl::PointXYZ> ); extract.filter( *cloud_plane ); // Remove the planar inliers, extract the rest pcl::PointCloud<pcl::PointXYZ>::Ptr filteredScene( new pcl::PointCloud<pcl::PointXYZ> ); extract.setNegative( true ); extract.filter( *filteredScene ); pcl::ExtractIndices<pcl::Normal> extract_normals; pcl::PointCloud<pcl::Normal>::Ptr filteredSceneNormals( new pcl::PointCloud<pcl::Normal> ); extract_normals.setNegative( true ); extract_normals.setInputCloud( pSceneNormals ); extract_normals.setIndices( inliers_plane ); extract_normals.filter( *filteredSceneNormals ); if( filteredScene->points.size() == 0 ) return; // --- Set Our Target Cloud --- // // Assign to the target FeatureCloud FeatureCloud target_cloud; target_cloud.setInputCloud( filteredScene ); // --- Visualize the Filtered Cloud --- // //visualize( filteredScene, visualizer_o_Ptr ); // --- Set Input Cloud For Alignment --- // template_align.setTargetCloud( target_cloud ); // --- Align Templates --- // std::cout << "Searching for best fit" << std::endl; // Find the best template alignment TemplateAlignment::Result best_alignment; int best_index = template_align.findBestAlignment( best_alignment ); std::cerr << "Best alignment index: " << best_index << std::endl; const FeatureCloud &best_template = object_templates[best_index]; // --- Report Best Match --- // // Print the alignment fitness score (values less than 0.00002 are good) std::cerr << "Best fitness score: " << best_alignment.fitness_score << std::endl; // Print the rotation matrix and translation vector Eigen::Matrix3f rotation = best_alignment.final_transformation.block<3,3>(0, 0); Eigen::Vector3f translation = best_alignment.final_transformation.block<3,1>(0, 3); std::cerr << std::setprecision(3); std::cerr << std::endl; std::cerr << " | " << rotation(0,0) << " " << rotation(0,1) << " " << rotation(0,2) << " | " << std::endl; std::cerr << "R = | " << rotation(1,0) << " " << rotation(1,1) << " " << rotation(1,2) << " | " << std::endl; std::cerr << " | " << rotation(2,0) << " " << rotation(2,1) << " " << rotation(2,2) << " | " << std::endl; std::cerr << std::endl; std::cerr << "t = < " << translation(0) << ", " << translation(1) << ", " << translation(2) << " >" << std::endl << std::endl; // pcl::PointCloud<pcl::PointXYZ> transformedCloud; // pcl::transformPointCloud( *best_template.getPointCloud(), transformedCloud, best_alignment.final_transformation); // visualize( filteredScene, transformedCloud.makeShared(), visualizer_o_Ptr ); // --- Publish --- // // TODO: Clean up this part. geometry_msgs::Pose pose; tf::Matrix3x3 rot( rotation(0,0), rotation(0,1), rotation(0,2), rotation(1,0), rotation(1,1), rotation(1,2), rotation(2,0), rotation(2,1), rotation(2,2) ); tf::Quaternion q; rot.getRotation(q); pose.orientation.w = q.getW(); pose.orientation.x = q.getX(); pose.orientation.y = q.getY(); pose.orientation.z = q.getZ(); tf::Vector3 t( translation(0), translation(1), translation(2) ); pose.position.x = t.getX(); pose.position.y = t.getY(); pose.position.z = t.getZ(); std::cerr << "Publishing" << std::endl; pub.publish(pose); sensor_msgs::PointCloud2 toPub; pcl::toROSMsg( *filteredScene, toPub ); cloud_pub.publish(toPub); }
// Ros kinect topic callback void callback(const sensor_msgs::PointCloud2ConstPtr& input) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloudKinect(new pcl::PointCloud<pcl::PointXYZ>()); pcl::fromROSMsg(*input, *cloudKinect); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>()); //std::cout << cloud << std::endl; //ROS_INFO(input); //Inside the callback should be all the process that needed to be done with the point cloud //pcl::PointCloud<pcl::PointXYZ> cloudKinect; //pcl::fromROSMsg(*input, cloudKinect); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(cloudKinect); //pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>); std::cout << cloud->points.size () << std::endl; // Create the filtering object: downsample the dataset using a leaf size of 1cm pcl::VoxelGrid<pcl::PointXYZ> vg; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>()); vg.setInputCloud (cloud); vg.setLeafSize (0.01f, 0.01f, 0.01f); vg.filter (*cloud_filtered); std::cout << *cloud << std::endl; // pcl::PassThrough<pcl::PointXYZ> pass; // pass.setInputCloud (cloud); // Create the segmentation object for the planar model and set all the parameters pcl::SACSegmentation<pcl::PointXYZ> seg; pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ()); pcl::PCDWriter writer; seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); seg.setDistanceThreshold (0.02); int i=0, nr_points = (int) cloud_filtered->points.size (); while (cloud_filtered->points.size () > 0.3 * nr_points) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cout << "Could not estimate a planar model for the given dataset." << std::endl; break; } // Extract the planar inliers from the input cloud pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud (cloud_filtered); extract.setIndices (inliers); extract.setNegative (false); // Get the points associated with the planar surface extract.filter (*cloud_plane); // Remove the planar inliers, extract the rest extract.setNegative (true); extract.filter (*cloud_f); *cloud_filtered = *cloud_f; } // Creating the KdTree object for the search method of the extraction pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); tree->setInputCloud (cloud_filtered); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; ec.setClusterTolerance (0.02); // 2cm ec.setMinClusterSize (100); ec.setMaxClusterSize (25000); ec.setSearchMethod (tree); ec.setInputCloud (cloud_filtered); ec.extract (cluster_indices); int j = 0; for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) { float minX = 1000000.0; float minY = 1000000.0; float maxX = -1000000.0; float maxY = -1000000.0; 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++){ pcl::PointXYZ p = cloud_filtered->points[*pit]; cloud_cluster->points.push_back (cloud_filtered->points[*pit]); if(p.x < minX){ minX = p.x; } if(p.x > maxX){ maxX = p.x; } if(p.y < minY){ minY = p.y; } if(p.y > maxY){ maxY = p.y; } } cloud_cluster->width = cloud_cluster->points.size (); cloud_cluster->height = 1; cloud_cluster->is_dense = true; Eigen::Vector4f centroid; pcl::compute3DCentroid(*cloud_cluster, centroid); // for (pcl::PointCloud<pcl::PointXYZ>::iterator p = cloud_cluster->points.begin(); p < cloud_cluster->points.end(); p++) // { // if(p.x < minX){ minX = p; } // if(p.x > maxX){ maxX = p; } // if(p.y < minY){ minY = p; } // if(p.y > maxY){ maxY = p; } // } // std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl; // std::cout << "Centroid " << centroid[0] << "," << centroid[1] << "," << centroid[2] << "." << std::endl; // std::cout << "min x: " << minX << std::endl; // std::cout << "max x: " << maxX << std::endl; // std::cout << "min y: " << minY << std::endl; // std::cout << "max y: " << maxY << std::endl; float xdist = std::abs(minX-maxY); float ydist = std::abs(minY-maxY); // std::cout << "dist x: " << xdist << std::endl; // std::cout << "dist y: " << ydist << std::endl; if(xdist < grippersize || ydist < grippersize){ // std::cout << "Graspable" << std::endl; std_msgs::String msg; std::stringstream rosmsg; rosmsg << centroid[0] << ","; rosmsg << centroid[1] << ","; rosmsg << centroid[2]; msg.data = rosmsg.str(); visionPublisher.publish(msg); std::stringstream ss; ss << "graspable_object" << j << ".pcd"; writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); j++; } } }