Last active
February 1, 2023 02:47
-
-
Save tanmayshankar/b8fe9806ec7f93c357d7 to your computer and use it in GitHub Desktop.
A ROS (Robot Operating System) adaptation of the Euclidean Cluster Extraction tutorial (http://www.pointclouds.org/documentation/tutorials/cluster_extraction.php#cluster-extraction)
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #include <ros/ros.h> | |
| #include <iostream> | |
| // PCL specific includes | |
| #include <pcl/ModelCoefficients.h> | |
| #include <pcl/point_types.h> | |
| #include <pcl/io/pcd_io.h> | |
| #include <pcl/filters/extract_indices.h> | |
| #include <pcl/filters/voxel_grid.h> | |
| #include <pcl/features/normal_3d.h> | |
| #include <pcl/kdtree/kdtree.h> | |
| #include <pcl/sample_consensus/method_types.h> | |
| #include <pcl/sample_consensus/model_types.h> | |
| #include <pcl/segmentation/sac_segmentation.h> | |
| #include <pcl/segmentation/extract_clusters.h> | |
| #include <boost/lexical_cast.hpp> | |
| // ros::Publisher pub; | |
| std::vector<ros::Publisher> pub_vec; | |
| sensor_msgs::PointCloud2::Ptr downsampled, output; | |
| // pcl::PointCloud<pcl::PointXYZ>::Ptr output_p, downsampled_XYZ; | |
| /* | |
| Example of pcl usage where we try to use clustering to detect objects | |
| in the scene. | |
| Original code taken from: | |
| http://www.pointclouds.org/documentation/tutorials/cluster_extraction.php#cluster-extraction | |
| Usage: | |
| first run: rosrun pcl_ros pcd_to_pointcloud <file.pcd> [ <interval> ] | |
| then run: rosrun my_pcl_tutorial cluster_extraction | |
| and look at the visualization through rviz with: | |
| rosrun rviz rviz | |
| Set /base_link as the Fixed Frame, check out the published PointCloud2 topics | |
| */ | |
| 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; | |
| } | |
| } | |
| int | |
| main (int argc, char** argv) | |
| { | |
| // Initialize ROS | |
| ros::init (argc, argv, "cluster_extraction"); | |
| ros::NodeHandle nh; | |
| // Create a ROS subscriber for the input point cloud | |
| ros::Subscriber sub = nh.subscribe ("/cloud_pcd", 1, cloud_cb); | |
| // Create a ROS publisher for the output point cloud | |
| // pub = nh.advertise<sensor_msgs::PointCloud2> ("/pcl_tut/object_cluster", 1); | |
| // Spin | |
| ros::spin (); | |
| } |
Thanks for the tutorial. Is anyone else also facing the same issue
"int pcl::KdTreeFLANN<PointT, Dist>::radiusSearch(const PointT&, double, std::vector&, std::vector&, unsigned int) const [with PointT = pcl::PointXYZ; Dist = flann::L2_Simple]: Assertion `point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!"' failed."
This was supposed to appear when the input cloud is not dense or has infinite or NaN values. Does anyone have solution for this ? thank you
PCL library provides pcl::removeNaNFromPointCloud (...) method to filter out NaN points.
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Thanks for the tutorial. Is anyone else also facing the same issue
"int pcl::KdTreeFLANN<PointT, Dist>::radiusSearch(const PointT&, double, std::vector&, std::vector&, unsigned int) const [with PointT = pcl::PointXYZ; Dist = flann::L2_Simple]: Assertion `point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!"' failed."
This was supposed to appear when the input cloud is not dense or has infinite or NaN values. Does anyone have solution for this ? thank you