// Callback Function for the subscribed ROS topic
void cloud_cb (const pcl::PCLPointCloud2ConstPtr& input) {
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromPCLPointCloud2(*input,*cloud);
// Create the filtering object: downsample the dataset using a leaf size of 0.5cm
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (input);
sor.setLeafSize (0.005f, 0.005f, 0.005f);
pcl::PCLPointCloud2 cloud_filtered;
sor.filter (cloud_filtered);
// Create the segmentation object
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.01);
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered2(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromPCLPointCloud2(cloud_filtered,*cloud_filtered2);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>);
seg.setInputCloud (cloud_filtered2);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
return;
}
// Extract the inliers
extract.setInputCloud (cloud_filtered2);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_p);
std::cout << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl;
// Publish the model coefficients
pcl::PCLPointCloud2 segmented_pcl;
pcl::toPCLPointCloud2(*cloud_p,segmented_pcl);
sensor_msgs::PointCloud2 segmented, not_segmented;
pcl_conversions::fromPCL(cloud_filtered,segmented);
pcl_conversions::fromPCL(*input,not_segmented);
pub.publish (segmented);
pub2.publish(not_segmented);
//Update PCL Viewer
printToPCLViewer();
}
void SimpleOpenNIViewer::cloud_cb_(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &cloud)
{
if (!viewer.wasStopped())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered2 (new pcl::PointCloud<pcl::PointXYZ>);
//FILTERING PROCESS TO ELIMINATE EVERYTHING BUT THE SURFACE
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, 1.3);
pass.filter (*cloud_filtered);
pass.setInputCloud (cloud_filtered);
pass.setFilterFieldName ("x");
pass.setFilterLimits (-0.4, 0.4);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_filtered);
pass.setInputCloud (cloud_filtered);
pass.setFilterFieldName ("y");
pass.setFilterLimits (-0.15, 0.3);
pass.filter (*cloud_filtered);
//DOWNSAMPLING RESULTING POINT CLOUD
pcl::VoxelGrid<pcl::PointXYZ> sor;
sor.setInputCloud (cloud_filtered);
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*cloud_filtered2);
//SEGMENT SURFACE
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);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (cloud_filtered2->makeShared());
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
PCL_ERROR ("Could not estimate a planar model for the given dataset.");
exit(0);
}
//PAINT SURFACE
/* for (unsigned int i = 0; i < inliers->indices.size(); i++)
{
int idx = inliers->indices[i];
cloud_filtered2->points[idx].r = 255;
cloud_filtered2->points[idx].g = 0;
cloud_filtered2->points[idx].b = 0;
} */
viewer.showCloud(cloud_filtered2);
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
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;
}
}
void cloud_callback(const sensor_msgs::PointCloud2ConstPtr& input) {
ROS_DEBUG("Got new pointcloud.");
// 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>);
std_msgs::Header header = input->header;
pcl::fromROSMsg(*input, *cloud);
//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> ());
//data sets
pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<PointT>::Ptr cloud_filtered2 (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients), coefficients_sphere (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices), inliers_sphere (new pcl::PointIndices);
ROS_DEBUG("PointCloud before filtering has: %i data points.", (int)cloud->points.size());
//filter our NaNs
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, 2.0);
pass.filter (*cloud_filtered);
ROS_DEBUG("PointCloud after filtering has: %i data points." , (int)cloud_filtered->points.size());
*cloud_filtered = *cloud;//remove the pass through filter basically
//estimate normal points
ne.setSearchMethod (tree);
ne.setInputCloud (cloud_filtered);
//ne.setKSearch(10);
ne.setRadiusSearch(0.025);
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.05);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (5.0);
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);
// 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);
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_SPHERE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setNormalDistanceWeight (0.1);
seg.setMaxIterations (100);
seg.setDistanceThreshold (5.0);
seg.setRadiusLimits (0, 1.0);
seg.setInputCloud (cloud_filtered2);
seg.setInputNormals (cloud_normals2);
extract.setInputCloud (cloud_filtered2);
extract.setIndices (inliers_sphere);
extract.setNegative (false);
pcl::PointCloud<PointT>::Ptr sphere_cloud (new pcl::PointCloud<PointT> ());
extract.filter(*sphere_cloud);
pcl::PointCloud<pcl::PointXYZ> sphere_cloud_in = *sphere_cloud;
sensor_msgs::PointCloud2 sphere_cloud_out;
pcl::toROSMsg(sphere_cloud_in, sphere_cloud_out);
sphere_cloud_out.header = header;
buoy_cloud_pub.publish(sphere_cloud_out);
}
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 PlaneFinder::pcCallback(const sensor_msgs::PointCloud2::ConstPtr & pc)
{
pcl::PCLPointCloud2::Ptr pcl_pc(new pcl::PCLPointCloud2);
pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered1 (new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered2 (new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered3 (new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PCLPointCloud2::Ptr cloud_out (new pcl::PCLPointCloud2 ());
sensor_msgs::PointCloud2 pc2;
double height = -0.5;
// Transformation into PCL type PointCloud2
pcl_conversions::toPCL(*(pc), *(pcl_pc));
//////////////////
// Voxel filter //
//////////////////
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (pcl_pc);
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*cloud_filtered);
// Transformation into ROS type
//pcl::toPCLPointCloud2(*(cloud_filtered2), *(cloud_out));
//pcl_conversions::moveFromPCL(*(cloud_filtered), pc2);
//debug2_pub.publish(pc2);
// Transformation into PCL type PointCloud<pcl::PointXYZRGB>
pcl::fromPCLPointCloud2(*(cloud_filtered), *(cloud_filtered1));
if(pcl_ros::transformPointCloud("map", *(cloud_filtered1), *(cloud_filtered2), tf_))
{
////////////////////////
// PassThrough filter //
////////////////////////
pcl::PassThrough<pcl::PointXYZRGB> pass;
pass.setInputCloud (cloud_filtered2);
pass.setFilterFieldName ("x");
pass.setFilterLimits (-0.003, 0.9);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_filtered2);
pass.setInputCloud (cloud_filtered2);
pass.setFilterFieldName ("y");
pass.setFilterLimits (-0.5, 0.5);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_filtered2);
/////////////////////////
// Planar segmentation //
/////////////////////////
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
//seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.01);
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
int i = 0, nr_points = (int) cloud_filtered2->points.size ();
// While 50% of the original cloud is still there
while (cloud_filtered2->points.size () > 0.5 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered2);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
if( (fabs(coefficients->values[0]) < 0.02) &&
(fabs(coefficients->values[1]) < 0.02) &&
(fabs(coefficients->values[2]) > 0.9) )
{
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
height = coefficients->values[3];
// Extract the inliers
extract.setInputCloud (cloud_filtered2);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_filtered3);
// Transformation into ROS type
//pcl::toPCLPointCloud2(*(cloud_filtered3), *(cloud_out));
//pcl_conversions::moveFromPCL(*(cloud_out), pc2);
//debug_pub.publish(pc2);
}
// Create the filtering object
extract.setNegative (true);
extract.filter (*cloud_f);
cloud_filtered2.swap (cloud_f);
i++;
}
/*
pass.setInputCloud (cloud_filtered2);
pass.setFilterFieldName ("z");
pass.setFilterLimits (height, 1.5);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_filtered2);
*/
// Transformation into ROS type
//pcl::toPCLPointCloud2(*(cloud_filtered2), *(cloud_out));
//pcl_conversions::moveFromPCL(*(cloud_out), pc2);
//debug_pub.publish(pc2);
/////////////////////////////////
// Statistical Outlier Removal //
/////////////////////////////////
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
sor.setInputCloud (cloud_filtered2);
sor.setMeanK (50);
sor.setStddevMulThresh (1.0);
sor.filter (*cloud_filtered2);
//////////////////////////////////
// Euclidian Cluster Extraction //
//////////////////////////////////
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud (cloud_filtered2);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (50);
ec.setMaxClusterSize (5000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_filtered2);
ec.extract (cluster_indices);
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster1 (new pcl::PointCloud<pcl::PointXYZRGB>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster1->points.push_back (cloud_filtered2->points[*pit]);
cloud_cluster1->width = cloud_cluster1->points.size ();
cloud_cluster1->height = 1;
cloud_cluster1->is_dense = true;
cloud_cluster1->header.frame_id = "/map";
if(j == 0) {
// Transformation into ROS type
pcl::toPCLPointCloud2(*(cloud_cluster1), *(cloud_out));
pcl_conversions::moveFromPCL(*(cloud_out), pc2);
debug_pub.publish(pc2);
}
else {
// Transformation into ROS type
pcl::toPCLPointCloud2(*(cloud_cluster1), *(cloud_out));
pcl_conversions::moveFromPCL(*(cloud_out), pc2);
debug2_pub.publish(pc2);
}
j++;
}
// Transformation into ROS type
//pcl::toPCLPointCloud2(*(cloud_filtered2), *(cloud_out));
//pcl_conversions::moveFromPCL(*(cloud_out), pc2);
//debug2_pub.publish(pc2);
//debug2_pub.publish(*pc_map);
}
}
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
std::vector<int> filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
if (filenames.size() == 0) {
PCL_ERROR ("No pcd files provided");
return 0;
}
if (pcl::io::loadPCDFile<PointType> (argv[filenames[0]], *cloud) == -1) {
PCL_ERROR ("Couldn't read file %s.pcd \n", argv[filenames[0]]);
}
std::cerr << "PointCloud has: " << cloud->points.size () << " data points." << std::endl;
// Estimate point normals
ne.setSearchMethod (tree);
ne.setInputCloud (cloud);
ne.setKSearch (10);
ne.compute (*cloud_normals);
// 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);
seg.setInputNormals (cloud_normals);
// 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);
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 PlanSegmentor::cloud_callback(const pcl::PCLPointCloud2ConstPtr &p_input)
{
// Create the filtering object: downsample the dataset using a leaf size of 0.5cm
pcl::PCLPointCloud2Ptr cloud_filtered = voxelgrid_filter(p_input, 0.005f);
// Passthrough filter
cloud_filtered = passthrough_filter(cloud_filtered,0,2.5);
// Transform pc2 to pc
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered2(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromPCLPointCloud2(*cloud_filtered,*cloud_filtered2);
// To show the filtered data in the pointcloud viewer
*m_cloud = *cloud_filtered2;
// Plane segmentation
m_segmented_cloud = plane_segmentation(cloud_filtered2,5);
if(m_segmented_cloud->size() == 0) return;
// std::cout << "PointCloud representing the planar components: " << segmented_cloud->width * segmented_cloud->height << " data points." << std::endl;
cloud_filtered2 = radius_outlier_removal_filter(cloud_filtered2,0.05,30);
// ============== Euclidean Object Clustering ============== //
// http://pointclouds.org/documentation/tutorials/cluster_extraction.php
// // Here, the extracted planes are not included in cloud_filtered2 (extracted by plane_segmentation function)
// std::vector<pcl::PointIndices> cluster_indices;
// euclidean_object_segmentation(cloud_filtered2,cluster_indices);
// std::cout << "Number of found objects: " << cluster_indices.size() << std::endl;
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr object_clusters (new pcl::PointCloud<pcl::PointXYZRGB>);
// for (int i=0; i<cluster_indices.size(); i++){
// pcl::PointIndices cloud_indices = cluster_indices.at(i);
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
// cloud_cluster = extract_object_from_indices(cloud_filtered2,cloud_indices);
// *object_clusters += *cloud_cluster;
// }
// objects_cloud.swap(object_clusters);
// =========================================================== //
// pc to pc2
pcl::PCLPointCloud2 planes_pcl,not_planes_pcl;
pcl::toPCLPointCloud2(*m_segmented_cloud,planes_pcl);
pcl::toPCLPointCloud2(*cloud_filtered2,not_planes_pcl);
//From PCL pointclouds to ROS pointclouds (sensor_msgs)
sensor_msgs::PointCloud2 filtered, planes,not_planes;
pcl_conversions::fromPCL(planes_pcl,planes);
pcl_conversions::fromPCL(*cloud_filtered,filtered);
pcl_conversions::fromPCL(not_planes_pcl,not_planes);
pcl_conversions::fromPCL(p_input->header,planes.header);
pcl_conversions::fromPCL(p_input->header,filtered.header);
pcl_conversions::fromPCL(p_input->header,not_planes.header);
// Publish on given topics
m_pub.publish (planes);
m_pub2.publish(filtered);
m_pub3.publish (not_planes);
//Update PCL Viewer
if(m_showUI){
printToPCLViewer();
}
}
int main (int argc, char** argv)
{
// Zmienne ktore uzywamy do segmentacji, filtracji, odczytu i zapisu pliku.
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> ());
pcl::visualization::CloudViewer viewer ("Cylinder Model Segmentation");
// Zmienne ktore przechowuja kolejno chmury naszych punktów
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);
// Wczytanie chmury punktów
reader.read ("test_pcd.pcd", *cloud);
std::cerr << "PointCloud has: " << cloud->points.size () << " data points." << std::endl;
// Przefiltorwanie chmury punktów w celu usuniecia "fa³szywych" punktów (NaN)
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, 2.8);
pass.filter (*cloud_filtered);
std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl;
// Obliczenie normalnych dla punktów w chmurze
ne.setSearchMethod (tree);
ne.setInputCloud (cloud_filtered);
ne.setKSearch (50);
ne.compute (*cloud_normals);
// Stworzenie obiektu do segmentacji planarnej, ustawienie odpowiednich parametrów
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);
// Segmentacja..
seg.segment (*inliers_plane, *coefficients_plane);
std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;
// Wy³uskanie obiektu ktory segmentowalismy
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers_plane);
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);
// Utworzenie obiektu do segmentacji cylindrycznej, ustawienie odpowiednich parametrów
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_CYLINDER);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setNormalDistanceWeight (0.1);
seg.setMaxIterations (10000);
// Maksymalna odleglosc pomiedzy punktam a prost¹ ktora wyznaczana jest przy segmentacji moze wynosic 33 cm
seg.setDistanceThreshold (0.33);
// Maksymalny promien cylindra moze miec 40 cm
seg.setRadiusLimits (0, 0.45);
seg.setInputCloud (cloud_filtered2);
seg.setInputNormals (cloud_normals2);
// Segmentacja...
seg.segment (*inliers_cylinder, *coefficients_cylinder);
std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;
// Zapisz wynik do pliku na dysku
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 ("test_pcd_cylinder.pcd", *cloud_cylinder, false);
viewer.showCloud (cloud_cylinder);
while (!viewer.wasStopped ())
{
}
}
return (0);
}
