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++;
}
}
}
