-
-
Save tanmayshankar/b8fe9806ec7f93c357d7 to your computer and use it in GitHub Desktop.
| #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 (); | |
| } |
Hey Tanmay,
thx for making this public. I'm having trouble getting it to run, once I do the catkin_make, I still get a bunch of error messages, would you mind sharing the structure of your CMake File too? (or the general project folder) , so I could try to get an exact copy running!
Thanks a lot
Here is the error code:
/home/mert/catkin_ws/src/clustering/src/cluster_extraction.cpp:19:1: error: ‘sensor_msgs’ does not name a type
sensor_msgs::PointCloud2::Ptr downsampled, output;
^~~~~~~~~~~
/home/mert/catkin_ws/src/clustering/src/cluster_extraction.cpp:41:17: error: ‘sensor_msgs’ does not name a type
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
^~~~~~~~~~~
/home/mert/catkin_ws/src/clustering/src/cluster_extraction.cpp:41:49: error: expected unqualified-id before ‘&’ token
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
^
/home/mert/catkin_ws/src/clustering/src/cluster_extraction.cpp:41:49: error: expected ‘)’ before ‘&’ token
/home/mert/catkin_ws/src/clustering/src/cluster_extraction.cpp:41:51: error: expected initializer before ‘input’
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
^~~~~
/home/mert/catkin_ws/src/clustering/src/cluster_extraction.cpp: In function ‘int main(int, char**)’:
/home/mert/catkin_ws/src/clustering/src/cluster_extraction.cpp:178:58: error: ‘cloud_cb’ was not declared in this scope
ros::Subscriber sub = nh.subscribe ("/cloud_pcd", 1, cloud_cb);
^~~~~~~~
/home/mert/catkin_ws/src/clustering/src/cluster_extraction.cpp:178:58: note: suggested alternative: ‘clock’
ros::Subscriber sub = nh.subscribe ("/cloud_pcd", 1, cloud_cb);
^~~~~~~~
clock
In file included from /home/mert/catkin_ws/src/clustering/src/cluster_extraction.cpp:12:0:
/usr/include/pcl-1.8/pcl/sample_consensus/model_types.h: In function ‘void __static_initialization_and_destruction_0(int, int)’:
/usr/include/pcl-1.8/pcl/sample_consensus/model_types.h:99:3: warning: ‘pcl::SAC_SAMPLE_SIZE’ is deprecated: This map is deprecated and is kept only to prevent breaking existing user code. Starting from PCL 1.8.0 model sample size is a protected member of the SampleConsensusModel class [-Wdeprecated-declarations]
SAC_SAMPLE_SIZE (sample_size_pairs, sample_size_pairs + sizeof (sample_size_pairs) / sizeof (SampleSizeModel));
^~~~~~~~~~~~~~~
/usr/include/pcl-1.8/pcl/sample_consensus/model_types.h:99:3: note: declared here
clustering/CMakeFiles/cluster_extraction.dir/build.make:62: recipe for target 'clustering/CMakeFiles/cluster_extraction.dir/src/cluster_extraction.cpp.o' failed
make[2]: *** [clustering/CMakeFiles/cluster_extraction.dir/src/cluster_extraction.cpp.o] Error 1
CMakeFiles/Makefile2:1084: recipe for target 'clustering/CMakeFiles/cluster_extraction.dir/all' failed
make[1]: *** [clustering/CMakeFiles/cluster_extraction.dir/all] Error 2
Makefile:140: recipe for target 'all' failed
make: *** [all] Error 2
Invoking "make -j4 -l4" failed
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
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.
Thank you for making this public! I found it useful. A comment:
Instead of creating a new nodeHandle in the callback (line 132), you can create a global pointer to a nodeHandle. Then just use the nodeHandle from main(). Like this:
ros::NodeHandle* nhPtr; // Give global access to the nodeHandle
void cloud_cb()
{
ros::Publisher pub = nhPtr->advertise<sensor_msgs::PointCloud2> (topicName, 1);
}
int main(int argc, char** argv)
{
ros init (...)
ros::NodeHandle nh;
nhPtr = & nh; // Give global access to this nodeHandle
}
If you're a purist who's worried about using global variables for nhPtr, you could put it in a namespace.