Пример #1
0
bool TabletopSegmentor<PointT>::processCloud(const PointCloudConstPtr &_cloud ) {

  // PCL objects
  KdTreePtr normals_tree_, clusters_tree_;
  pcl::VoxelGrid<PointT> grid_, grid_objects_;
  pcl::PassThrough<PointT> pass_;
  pcl::NormalEstimation<PointT, pcl::Normal> n3d_;
  pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg_;
  pcl::ProjectInliers<PointT> proj_;
  pcl::ConvexHull<PointT> hull_;
  pcl::ExtractPolygonalPrismData<PointT> prism_;
  pcl::EuclideanClusterExtraction<PointT> pcl_cluster_;
  PointCloudPtr table_hull_ptr (new PointCloud);

  /************ STEP 0: Parameter Settings **************/
  // Filtering parameters
  grid_.setLeafSize (plane_detection_voxel_size_, plane_detection_voxel_size_, plane_detection_voxel_size_);
  grid_objects_.setLeafSize (clustering_voxel_size_, clustering_voxel_size_, clustering_voxel_size_);
  grid_.setFilterFieldName ("z");
  grid_.setFilterLimits (z_filter_min_, z_filter_max_);
  grid_.setDownsampleAllData (false);
  grid_objects_.setDownsampleAllData (true);
  normals_tree_ = boost::make_shared<pcl::search::KdTree<PointT> > ();
  clusters_tree_ = boost::make_shared<pcl::search::KdTree<PointT> > ();
  
  // Normal estimation parameters
  n3d_.setKSearch (10);
  n3d_.setSearchMethod (normals_tree_);

  // Table model fitting parameters
  seg_.setDistanceThreshold (0.01);
  seg_.setMaxIterations (10000);
  seg_.setNormalDistanceWeight (0.1);
  seg_.setOptimizeCoefficients (true);
  seg_.setModelType (pcl::SACMODEL_PLANE);
  //seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
  //seg_.setModelType( pcl::SACMODEL_PERPENDICULAR_PLANE );
  //seg_.setEpsAngle( 45*M_PI/180.0 );
  //seg_.setAxis( Eigen::Vector3f(0,1,0) );
  
  seg_.setMethodType (pcl::SAC_RANSAC);
  seg_.setProbability (0.99);
  
  //proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
  proj_.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
  
  
  // Clustering parameters
  pcl_cluster_.setClusterTolerance (cluster_distance_);
  pcl_cluster_.setMinClusterSize (min_cluster_size_);
  pcl_cluster_.setSearchMethod (clusters_tree_);

  /******** Step 1 : Filter, remove NaNs and downsample ***********/
  // Filter in X, Y and Z directions: output= cloud_filtered_ptr
  pass_.setInputCloud (_cloud);
  pass_.setFilterFieldName ("z");
  pass_.setFilterLimits (z_filter_min_, z_filter_max_);
  PointCloudPtr z_cloud_filtered_ptr (new PointCloud);
  pass_.filter (*z_cloud_filtered_ptr);
  pass_.setInputCloud (z_cloud_filtered_ptr);
  pass_.setFilterFieldName ("y");
  pass_.setFilterLimits (y_filter_min_, y_filter_max_);
  PointCloudPtr y_cloud_filtered_ptr (new PointCloud);
  pass_.filter (*y_cloud_filtered_ptr);
  pass_.setInputCloud (y_cloud_filtered_ptr);
  pass_.setFilterFieldName ("x");
  pass_.setFilterLimits (x_filter_min_, x_filter_max_);
  PointCloudPtr cloud_filtered_ptr (new PointCloud);
  pass_.filter (*cloud_filtered_ptr);
  // Check that the points filtered at least are of a minimum size
  if (cloud_filtered_ptr->points.size() < (unsigned int)min_cluster_size_) {
    printf("\t [ERROR] Filtered cloud only has %d points. \n",
	   (int)cloud_filtered_ptr->points.size() );
    return false;
  }

  pcl::io::savePCDFile( "filtered_cloud.pcd", *cloud_filtered_ptr, true );
  
  // Downsample the filtered cloud: output = cloud_downsampled_ptr
  PointCloudPtr cloud_downsampled_ptr (new PointCloud);
  grid_.setInputCloud (cloud_filtered_ptr);
  grid_.filter (*cloud_downsampled_ptr);
  if (cloud_downsampled_ptr->points.size() < (unsigned int)min_cluster_size_) {
    printf( "\t [ERROR] Downsampled cloud only has %d points \n",
	    (int)cloud_downsampled_ptr->points.size() );
    return false;
  }

  printf( "\t Downsampled cloud only has %d points / %d. \n",
	  (int)cloud_downsampled_ptr->points.size(), (int)cloud_filtered_ptr->points.size() );


  /***************** Step 2 : Estimate normals ******************/
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ptr (new pcl::PointCloud<pcl::Normal>);
  n3d_.setInputCloud (cloud_downsampled_ptr);
  n3d_.compute (*cloud_normals_ptr);

  /************* Step 3 : Perform planar segmentation, **********/
  /** if table is not given, otherwise use given table */
  Eigen::Matrix4d table_plane_trans;
  Eigen::Matrix4d table_plane_trans_flat;
  pcl::PointIndices::Ptr table_inliers_ptr (new pcl::PointIndices);
  pcl::ModelCoefficients::Ptr table_coefficients_ptr (new pcl::ModelCoefficients);
  seg_.setInputCloud (cloud_downsampled_ptr);
  seg_.setInputNormals (cloud_normals_ptr);
  seg_.segment( *table_inliers_ptr,
		*table_coefficients_ptr );
  // Check the coefficients and inliers are above threshold values
  if (table_coefficients_ptr->values.size () <=3 ) {
    printf( "\t [ERROR] Failed to detect table in scan \n" );
    return false;
  }
  if ( table_inliers_ptr->indices.size() < (unsigned int)inlier_threshold_) {
    printf( "\t [ERROR] Plane detection has %d below min thresh of %d points \n",
	    (int)table_inliers_ptr->indices.size(),
	    inlier_threshold_ );
    return false;
  }

  pcl::ExtractIndices<PointT> extract;
  PointCloudPtr tablePointsPtr( new PointCloud );
  extract.setInputCloud( cloud_downsampled_ptr );
  extract.setIndices( table_inliers_ptr );
  extract.setNegative(false);
  extract.filter( *tablePointsPtr );
  int na = tablePointsPtr->points.size();
  if( na > 0 ) {
    for( int a = 0; a < na; ++a ) {
      tablePointsPtr->points[a].r = 255;
      tablePointsPtr->points[a].g = 255;
      tablePointsPtr->points[a].b = 255;
      tablePointsPtr->points[a].a = 255;
    }
    pcl::io::savePCDFileASCII( "table_points.pcd", *tablePointsPtr );

  }
  

  /********** Step 4 : Project the table inliers on the table *********/
  PointCloudPtr table_projected_ptr (new PointCloud);
  proj_.setInputCloud (cloud_downsampled_ptr);
  proj_.setIndices (table_inliers_ptr);
  proj_.setModelCoefficients (table_coefficients_ptr);
  proj_.filter (*table_projected_ptr);
  // Get the table transformation w.r.t. camera
  table_plane_trans = getPlaneTransform (*table_coefficients_ptr, up_direction_, false);
  mTableTf.matrix() = table_plane_trans;
  // ---[ Estimate the convex hull (not in table frame)
  hull_.setInputCloud (table_projected_ptr);
  hull_.reconstruct (*table_hull_ptr);

  // Save points in the hull with some points down
  mTable_Points.points.resize(0);
  for( int i = 0; i < table_hull_ptr->points.size(); ++i ) {
    mTable_Points.points.push_back(table_hull_ptr->points[i]);
  }
  mTable_Points.width = 1; mTable_Points.height = mTable_Points.points.size();
  
  // Store table's plane coefficients (after calling getPlaneTransform! because here we check if a sign change is needed)
  mTableCoeffs.resize(4);
  for( int i = 0; i < 4; ++i ) { mTableCoeffs[i] = table_coefficients_ptr->values[i]; }

  /******* Step 5 : Get the objects on top of the table ******/
  pcl::PointIndices cloud_object_indices;
  prism_.setInputCloud (cloud_filtered_ptr);
  prism_.setInputPlanarHull (table_hull_ptr);
  prism_.setHeightLimits (table_obj_height_filter_min_, table_obj_height_filter_max_);
  prism_.segment (cloud_object_indices);
  PointCloudPtr cloud_objects_ptr (new PointCloud);
  pcl::ExtractIndices<PointT> extract_object_indices;
  extract_object_indices.setInputCloud (cloud_filtered_ptr);
  extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
  extract_object_indices.filter (*cloud_objects_ptr);
  if (cloud_objects_ptr->points.empty ()) {
    std::cout<<"\t [ERROR] No object points on table" << std::endl;
    return false;
  }

  pcl::io::savePCDFile( "cloudObjects.pcd", *cloud_objects_ptr, true );    

  // Downsample the points
  PointCloudPtr cloud_objects_downsampled_ptr (new PointCloud);
  grid_objects_.setInputCloud (cloud_objects_ptr);
  grid_objects_.filter (*cloud_objects_downsampled_ptr);
  printf("Object clouds from %ld to %ld \n", cloud_objects_ptr->points.size(),
	 cloud_objects_downsampled_ptr->points.size() );
  // Step 6: Split the objects into Euclidean clusters
  std::vector<pcl::PointIndices> clusters2;
  pcl_cluster_.setInputCloud (cloud_objects_downsampled_ptr);
  pcl_cluster_.extract (clusters2);
  mClusterInds.resize( clusters2.size() );
  for( int i = 0; i < clusters2.size(); ++i ) {
    mClusterInds[i] = clusters2[i];
    printf("Cluster %d size: %ld \n", i, clusters2[i].indices.size()   );
  }

  mClusters.resize( clusters2.size() );
  int i = 0;
  for( std::vector<pcl::PointIndices>::const_iterator it = 
	 clusters2.begin (); it != clusters2.end (); ++it ) {
    PointCloud cloud_cluster;
    for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit) {
      cloud_cluster.points.push_back (cloud_objects_downsampled_ptr->points[*pit]); 
    }
    cloud_cluster.width = cloud_cluster.points.size ();
    cloud_cluster.height = 1;
    cloud_cluster.is_dense = true;
    
    mClusters[i] = cloud_cluster;
    i++;
  }
  
  return true;
}
void TabletopSegmentor::processCloud(pcl::PointCloud<Point>::Ptr &cloud_ptr,
                                     TabletopSegmentation::Response &response)
{
  //ROS_INFO("Starting process on new cloud in frame %s", cloud_ptr->header.frame_id.c_str());

  // PCL objects
  KdTreePtr normals_tree_, clusters_tree_;
  pcl::VoxelGrid<Point> grid_, grid_objects_;
  pcl::PassThrough<Point> pass_;
  pcl::NormalEstimation<Point, pcl::Normal> n3d_;
  pcl::SACSegmentationFromNormals<Point, pcl::Normal> seg_;
  pcl::ProjectInliers<Point> proj_;
  pcl::ConvexHull<Point> hull_;
  pcl::ExtractPolygonalPrismData<Point> prism_;
  pcl::EuclideanClusterExtraction<Point> pcl_cluster_;

  // Filtering parameters
  grid_.setLeafSize (plane_detection_voxel_size_, plane_detection_voxel_size_, plane_detection_voxel_size_);
  grid_objects_.setLeafSize (clustering_voxel_size_, clustering_voxel_size_, clustering_voxel_size_);
  grid_.setFilterFieldName ("z");
  grid_.setFilterLimits (z_filter_min_, z_filter_max_);
  grid_.setDownsampleAllData (false);
  grid_objects_.setDownsampleAllData (false);

  normals_tree_ = boost::make_shared<pcl::search::KdTree<Point> > ();
  clusters_tree_ = boost::make_shared<pcl::search::KdTree<Point> > ();

  // Normal estimation parameters
  n3d_.setKSearch (10);  
  n3d_.setSearchMethod (normals_tree_);
  // Table model fitting parameters
  seg_.setDistanceThreshold (0.05); 
  seg_.setMaxIterations (10000);
  seg_.setNormalDistanceWeight (0.1);
  seg_.setOptimizeCoefficients (true);
  seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
  seg_.setMethodType (pcl::SAC_RANSAC);
  seg_.setProbability (0.99);

  proj_.setModelType (pcl::SACMODEL_PLANE);

  // Clustering parameters
  pcl_cluster_.setClusterTolerance (cluster_distance_);
  pcl_cluster_.setMinClusterSize (min_cluster_size_);
  pcl_cluster_.setSearchMethod (clusters_tree_);

  // Step 1 : Filter, remove NaNs and downsample
  pass_.setInputCloud (cloud_ptr);
  pass_.setFilterFieldName ("z");
  pass_.setFilterLimits (z_filter_min_, z_filter_max_);
  pcl::PointCloud<Point>::Ptr z_cloud_filtered_ptr (new pcl::PointCloud<Point>); 
  pass_.filter (*z_cloud_filtered_ptr);

  pass_.setInputCloud (z_cloud_filtered_ptr);
  pass_.setFilterFieldName ("y");
  pass_.setFilterLimits (y_filter_min_, y_filter_max_);
  pcl::PointCloud<Point>::Ptr y_cloud_filtered_ptr (new pcl::PointCloud<Point>); 
  pass_.filter (*y_cloud_filtered_ptr);

  pass_.setInputCloud (y_cloud_filtered_ptr);
  pass_.setFilterFieldName ("x");
  pass_.setFilterLimits (x_filter_min_, x_filter_max_);
  pcl::PointCloud<Point>::Ptr cloud_filtered_ptr (new pcl::PointCloud<Point>); 
  pass_.filter (*cloud_filtered_ptr);
  
  //ROS_INFO("Step 1 done");
  if (cloud_filtered_ptr->points.size() < (unsigned int)min_cluster_size_)
  {
    //ROS_INFO("Filtered cloud only has %d points", (int)cloud_filtered_ptr->points.size());
    response.result = response.NO_TABLE;
    return;
  }

  pcl::PointCloud<Point>::Ptr cloud_downsampled_ptr (new pcl::PointCloud<Point>); 
  grid_.setInputCloud (cloud_filtered_ptr);
  grid_.filter (*cloud_downsampled_ptr);
  if (cloud_downsampled_ptr->points.size() < (unsigned int)min_cluster_size_)
  {
    //ROS_INFO("Downsampled cloud only has %d points", (int)cloud_downsampled_ptr->points.size());
    response.result = response.NO_TABLE;    
    return;
  }

  // Step 2 : Estimate normals
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ptr (new pcl::PointCloud<pcl::Normal>); 
  n3d_.setInputCloud (cloud_downsampled_ptr);
  n3d_.compute (*cloud_normals_ptr);
  //ROS_INFO("Step 2 done");

  // Step 3 : Perform planar segmentation
  pcl::PointIndices::Ptr table_inliers_ptr (new pcl::PointIndices); 
  pcl::ModelCoefficients::Ptr table_coefficients_ptr (new pcl::ModelCoefficients); 
  seg_.setInputCloud (cloud_downsampled_ptr);
  seg_.setInputNormals (cloud_normals_ptr);
  seg_.segment (*table_inliers_ptr, *table_coefficients_ptr);
 
  if (table_coefficients_ptr->values.size () <=3)
  {
    //ROS_INFO("Failed to detect table in scan");
    response.result = response.NO_TABLE;    
    return;
  }

  if ( table_inliers_ptr->indices.size() < (unsigned int)inlier_threshold_)
  {
    //ROS_INFO("Plane detection has %d inliers, below min threshold of %d", (int)table_inliers_ptr->indices.size(),inlier_threshold_);
    response.result = response.NO_TABLE;
    return;
  }

  //ROS_INFO ("[TableObjectDetector::input_callback] Model found with %d inliers: [%f %f %f %f].", 	    (int)table_inliers_ptr->indices.size (),	    table_coefficients_ptr->values[0], table_coefficients_ptr->values[1], 	    table_coefficients_ptr->values[2], table_coefficients_ptr->values[3]);
  //ROS_INFO("Step 3 done");

  // Step 4 : Project the table inliers on the table
  pcl::PointCloud<Point>::Ptr table_projected_ptr (new pcl::PointCloud<Point>); 
  proj_.setInputCloud (cloud_downsampled_ptr);
  proj_.setIndices (table_inliers_ptr);
  proj_.setModelCoefficients (table_coefficients_ptr);
  proj_.filter (*table_projected_ptr);
  //ROS_INFO("Step 4 done");
  
  sensor_msgs::PointCloud table_points;
  sensor_msgs::PointCloud table_hull_points;
  tf::Transform table_plane_trans = getPlaneTransform (*table_coefficients_ptr, up_direction_, false);
  tf::Transform table_plane_trans_flat;

  // ---[ Estimate the convex hull (not in table frame)
  pcl::PointCloud<Point>::Ptr table_hull_ptr (new pcl::PointCloud<Point>); 
  hull_.setInputCloud (table_projected_ptr);
  hull_.reconstruct (*table_hull_ptr);

  if(!flatten_table_)
  {
    // --- [ Take the points projected on the table and transform them into the PointCloud message
    //  while also transforming them into the table's coordinate system
    if (!getPlanePoints<Point> (*table_projected_ptr, table_plane_trans, table_points))
    {
      response.result = response.OTHER_ERROR;
      return;
    }

    // ---[ Create the table message
    response.table = getTable<sensor_msgs::PointCloud>(cloud_ptr->header, table_plane_trans, table_points);

    // ---[ Convert the convex hull points to table frame
    if (!getPlanePoints<Point> (*table_hull_ptr, table_plane_trans, table_hull_points))
    {
      response.result = response.OTHER_ERROR;
      return;
    }
  }
  if(flatten_table_)
  {
    // if flattening the table, find the center of the convex hull and move the table frame there
    table_plane_trans_flat = getPlaneTransform (*table_coefficients_ptr, up_direction_, flatten_table_);
    tf::Vector3 flat_table_pos;
    double avg_x, avg_y, avg_z;
    avg_x = avg_y = avg_z = 0;
    for (size_t i=0; i<table_projected_ptr->points.size(); i++)
    {
      avg_x += table_projected_ptr->points[i].x;
      avg_y += table_projected_ptr->points[i].y;
      avg_z += table_projected_ptr->points[i].z;
    }
    avg_x /= table_projected_ptr->points.size();
    avg_y /= table_projected_ptr->points.size();
    avg_z /= table_projected_ptr->points.size();
    //ROS_INFO("average x,y,z = (%.5f, %.5f, %.5f)", avg_x, avg_y, avg_z);

    // place the new table frame in the center of the convex hull
    flat_table_pos[0] = avg_x;
    flat_table_pos[1] = avg_y;
    flat_table_pos[2] = avg_z;
    table_plane_trans_flat.setOrigin(flat_table_pos);

    // shift the non-flat table frame to the center of the convex hull as well
    table_plane_trans.setOrigin(flat_table_pos);

    // --- [ Take the points projected on the table and transform them into the PointCloud message
    //  while also transforming them into the flat table's coordinate system
    sensor_msgs::PointCloud flat_table_points;
    if (!getPlanePoints<Point> (*table_projected_ptr, table_plane_trans_flat, flat_table_points))
    {
      response.result = response.OTHER_ERROR;
      return;
    }

    // ---[ Create the table message
    response.table = getTable<sensor_msgs::PointCloud>(cloud_ptr->header, table_plane_trans_flat, flat_table_points);

    // ---[ Convert the convex hull points to flat table frame
    if (!getPlanePoints<Point> (*table_hull_ptr, table_plane_trans_flat, table_hull_points))
    {
      response.result = response.OTHER_ERROR;
      return;
    }
  }

  //ROS_INFO("Table computed");
  response.result = response.SUCCESS;
  
  // ---[ Add the convex hull as a triangle mesh to the Table message
//  addConvexHullTable<sensor_msgs::PointCloud>(response.table, table_hull_points, flatten_table_);

  // ---[ Get the objects on top of the (non-flat) table
  pcl::PointIndices cloud_object_indices;
  prism_.setInputCloud (cloud_filtered_ptr);
  prism_.setInputPlanarHull (table_hull_ptr);
  //ROS_INFO("Using table prism: %f to %f", table_z_filter_min_, table_z_filter_max_);
  prism_.setHeightLimits (table_z_filter_min_, table_z_filter_max_);  
  prism_.segment (cloud_object_indices);

  pcl::PointCloud<Point>::Ptr cloud_objects_ptr (new pcl::PointCloud<Point>); 
  pcl::ExtractIndices<Point> extract_object_indices;
  extract_object_indices.setInputCloud (cloud_filtered_ptr);
  extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
  extract_object_indices.filter (*cloud_objects_ptr);

  //ROS_INFO (" Number of object point candidates: %d.", (int)cloud_objects_ptr->points.size ());

  if (cloud_objects_ptr->points.empty ()) 
  {
    //ROS_INFO("No objects on table");
    return;
  }

  // ---[ Downsample the points
  pcl::PointCloud<Point>::Ptr cloud_objects_downsampled_ptr (new pcl::PointCloud<Point>); 
  grid_objects_.setInputCloud (cloud_objects_ptr);
  grid_objects_.filter (*cloud_objects_downsampled_ptr);

  // ---[ Split the objects into Euclidean clusters
  std::vector<pcl::PointIndices> clusters2;
  pcl_cluster_.setInputCloud (cloud_objects_ptr);
  //pcl_cluster_.setInputCloud (cloud_objects_downsampled_ptr);
  pcl_cluster_.extract (clusters2);
  //:ROS_INFO ("Number of clusters found matching the given constraints: %d.", (int)clusters2.size ());

  // ---[ Convert clusters into the PointCloud message
  std::vector<sensor_msgs::PointCloud2> clusters;
  getClusters<Point> (*cloud_objects_ptr, clusters2, clusters);
  //ROS_INFO("Clusters converted");
  response.clusters = clusters;  

//  publishClusterMarkers(clusters, cloud.header);
}