tabletop_segmentation.cpp
Go to the documentation of this file.
00001 
00002 /*********************************************************************
00003 *
00004 *  Copyright (c) 2009, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034   
00035 // Author(s): Marius Muja and Matei Ciocarlie
00036 
00037 #include <string>
00038 
00039 #include <ros/ros.h>
00040 
00041 #include <sensor_msgs/PointCloud.h>
00042 #include <sensor_msgs/PointCloud2.h>
00043 #include <sensor_msgs/point_cloud_conversion.h>
00044 #include <visualization_msgs/Marker.h>
00045 
00046 #include <tf/transform_listener.h>
00047 #include <tf/transform_broadcaster.h>
00048 #include <tf/transform_datatypes.h>
00049 
00050 #include <pcl/point_cloud.h>
00051 #include <pcl/point_types.h>
00052 #include <pcl/io/io.h>
00053 #include <pcl/filters/voxel_grid.h>
00054 #include <pcl/filters/passthrough.h>
00055 #include <pcl/filters/extract_indices.h>
00056 #include <pcl/features/normal_3d.h>
00057 #include <pcl/sample_consensus/method_types.h>
00058 #include <pcl/sample_consensus/model_types.h>
00059 #include <pcl/segmentation/sac_segmentation.h>
00060 #include <pcl/filters/project_inliers.h>
00061 #include <pcl/surface/convex_hull.h>
00062 #include <pcl/search/kdtree.h>
00063 #include <pcl/segmentation/extract_polygonal_prism_data.h>
00064 #include <pcl/segmentation/extract_clusters.h>
00065 #include <pcl_ros/transforms.h>
00066 
00067 #include "tabletop_object_detector/marker_generator.h"
00068 #include "tabletop_object_detector/TabletopSegmentation.h"
00069 
00070 namespace tabletop_object_detector {
00071 
00072 class TabletopSegmentor 
00073 {
00074   typedef pcl::PointXYZRGB    Point;
00075   typedef pcl::search::KdTree<Point>::Ptr KdTreePtr;
00076   
00077 private:
00079   ros::NodeHandle nh_;
00081   ros::NodeHandle priv_nh_;
00083   ros::Publisher marker_pub_;
00085   ros::ServiceServer segmentation_srv_;
00086 
00088   int num_markers_published_;
00090   int current_marker_id_;
00091 
00093   int inlier_threshold_;
00095   double plane_detection_voxel_size_;
00097   double clustering_voxel_size_;
00099   double z_filter_min_, z_filter_max_;
00100   double y_filter_min_, y_filter_max_;
00101   double x_filter_min_, x_filter_max_;
00103   double table_z_filter_min_, table_z_filter_max_;
00105   double cluster_distance_;
00107   int min_cluster_size_;
00110   std::string processing_frame_;
00112   double up_direction_;
00113   bool flatten_table_;
00115   double table_padding_;
00116 
00118   tf::TransformListener listener_;
00119 
00120   //------------------ Callbacks -------------------
00121 
00123   bool serviceCallback(TabletopSegmentation::Request &request, TabletopSegmentation::Response &response);
00124 
00125   //------------------ Individual processing steps -------
00126 
00128   template <class PointCloudType>
00129   Table getTable(std_msgs::Header cloud_header, const tf::Transform &table_plane_trans,
00130                  const PointCloudType &table_points);
00131 
00133   template <class PointCloudType>
00134   void addConvexHullTable(Table &table, const PointCloudType &convex_hull, bool flatten_table);
00135 
00137   template <class PointCloudType>
00138   void publishClusterMarkers(const std::vector<PointCloudType> &clusters, std_msgs::Header cloud_header);
00139   
00140   //------------------- Complete processing -----
00141 
00143   void processCloud(const sensor_msgs::PointCloud2 &cloud,
00144                     TabletopSegmentation::Response &response, 
00145                     Table table);
00146   
00148   void clearOldMarkers(std::string frame_id);
00149 
00151   template <class PointCloudType>
00152   bool tableMsgToPointCloud (Table &table, std::string frame_id, PointCloudType &table_hull);
00153 
00154 public:
00156 
00157   TabletopSegmentor(ros::NodeHandle nh) : nh_(nh), priv_nh_("~")
00158   {
00159     num_markers_published_ = 1;
00160     current_marker_id_ = 1;
00161 
00162     marker_pub_ = nh_.advertise<visualization_msgs::Marker>(nh_.resolveName("markers_out"), 10);
00163 
00164     segmentation_srv_ = nh_.advertiseService(nh_.resolveName("segmentation_srv"), 
00165                                              &TabletopSegmentor::serviceCallback, this);
00166 
00167     //initialize operational flags
00168     priv_nh_.param<int>("inlier_threshold", inlier_threshold_, 300);
00169     priv_nh_.param<double>("plane_detection_voxel_size", plane_detection_voxel_size_, 0.01);
00170     priv_nh_.param<double>("clustering_voxel_size", clustering_voxel_size_, 0.003);
00171     priv_nh_.param<double>("z_filter_min", z_filter_min_, 0.4);
00172     priv_nh_.param<double>("z_filter_max", z_filter_max_, 1.25);
00173     priv_nh_.param<double>("y_filter_min", y_filter_min_, -1.0);
00174     priv_nh_.param<double>("y_filter_max", y_filter_max_, 1.0);
00175     priv_nh_.param<double>("x_filter_min", x_filter_min_, -1.0);
00176     priv_nh_.param<double>("x_filter_max", x_filter_max_, 1.0);
00177     priv_nh_.param<double>("table_z_filter_min", table_z_filter_min_, 0.01);
00178     priv_nh_.param<double>("table_z_filter_max", table_z_filter_max_, 0.50);
00179     priv_nh_.param<double>("cluster_distance", cluster_distance_, 0.03);
00180     priv_nh_.param<int>("min_cluster_size", min_cluster_size_, 300);
00181     priv_nh_.param<std::string>("processing_frame", processing_frame_, "");
00182     priv_nh_.param<double>("up_direction", up_direction_, -1.0);   
00183     priv_nh_.param<bool>("flatten_table", flatten_table_, false);   
00184     priv_nh_.param<double>("table_padding", table_padding_, 0.0);   
00185     if(flatten_table_) ROS_DEBUG("flatten_table is true");
00186     else ROS_DEBUG("flatten_table is false");
00187 
00188   }
00189 
00191   ~TabletopSegmentor() {}
00192 };
00193 
00196 bool TabletopSegmentor::serviceCallback(TabletopSegmentation::Request &request, 
00197                                         TabletopSegmentation::Response &response)
00198 {
00199   ros::Time start_time = ros::Time::now();
00200   std::string topic = nh_.resolveName("cloud_in");
00201   ROS_INFO("Tabletop detection service called; waiting for a point_cloud2 on topic %s", topic.c_str());
00202 
00203   sensor_msgs::PointCloud2::ConstPtr recent_cloud = 
00204     ros::topic::waitForMessage<sensor_msgs::PointCloud2>(topic, nh_, ros::Duration(3.0));
00205               
00206   if (!recent_cloud)
00207   {
00208     ROS_ERROR("Tabletop object detector: no point_cloud2 has been received");
00209     response.result = response.NO_CLOUD_RECEIVED;
00210     return true;
00211   }
00212 
00213   pcl::PointCloud<Point>::Ptr table_hull (new pcl::PointCloud<Point>);
00214   ROS_INFO_STREAM("Point cloud received after " << ros::Time::now() - start_time << " seconds; processing");
00215   if (!processing_frame_.empty())
00216   {
00217     //convert cloud to processing_frame_ (usually base_link)
00218     sensor_msgs::PointCloud old_cloud;  
00219     sensor_msgs::convertPointCloud2ToPointCloud (*recent_cloud, old_cloud);
00220     int current_try=0, max_tries = 3;
00221     while (1)
00222     {
00223       bool transform_success = true;
00224       try
00225       {
00226         listener_.transformPointCloud(processing_frame_, old_cloud, old_cloud);    
00227       }
00228       catch (tf::TransformException ex)
00229       {
00230         transform_success = false;
00231         if (++current_try >= max_tries)
00232         {
00233           ROS_ERROR("Failed to transform cloud from frame %s into frame %s in %d attempt(s)", old_cloud.header.frame_id.c_str(), 
00234                     processing_frame_.c_str(), current_try);
00235           response.result = response.OTHER_ERROR;
00236           return true;
00237         }
00238         ROS_DEBUG("Failed to transform point cloud, attempt %d out of %d, exception: %s", current_try, max_tries, ex.what());
00239         //sleep a bit to give the listener a chance to get a new transform
00240         ros::Duration(0.1).sleep();
00241       }
00242       if (transform_success) break;
00243     }
00244     sensor_msgs::PointCloud2 converted_cloud;
00245     sensor_msgs::convertPointCloudToPointCloud2 (old_cloud, converted_cloud);
00246     ROS_INFO_STREAM("Input cloud converted to " << processing_frame_ << " frame after " <<
00247                     ros::Time::now() - start_time << " seconds");
00248     processCloud(converted_cloud, response, request.table);
00249     clearOldMarkers(converted_cloud.header.frame_id);
00250   }
00251   else
00252   {
00253     processCloud(*recent_cloud, response, request.table);
00254     clearOldMarkers(recent_cloud->header.frame_id);
00255   }
00256 
00257   //add the timestamp from the original cloud
00258   response.table.pose.header.stamp = recent_cloud->header.stamp;
00259   for(size_t i; i<response.clusters.size(); i++)
00260   {
00261     response.clusters[i].header.stamp = recent_cloud->header.stamp;
00262   }
00263 
00264   ROS_INFO_STREAM("In total, segmentation took " << ros::Time::now() - start_time << " seconds");
00265   return true;
00266 }
00267 
00268 template <class PointCloudType>
00269 void TabletopSegmentor::addConvexHullTable(Table &table,
00270                                            const PointCloudType &convex_hull,
00271                                            bool flatten_table)
00272 {
00273   if (convex_hull.points.empty())
00274   {
00275     ROS_ERROR("Trying to add convex hull, but it contains no points");
00276     return;
00277   }
00278   //compute centroid
00279   geometry_msgs::Point centroid;
00280   centroid.x = centroid.y = centroid.z = 0.0;
00281   for (size_t i=0; i<convex_hull.points.size(); i++)
00282   {
00283     centroid.x += convex_hull.points[i].x;
00284     centroid.y += convex_hull.points[i].y;
00285     centroid.z += convex_hull.points[i].z;
00286   }
00287   centroid.x /= convex_hull.points.size();
00288   centroid.y /= convex_hull.points.size();
00289   centroid.z /= convex_hull.points.size();
00290 
00291   //create a triangle mesh out of the convex hull points and add it to the table message
00292   for (size_t i=0; i<convex_hull.points.size(); i++)
00293   {
00294     geometry_msgs::Point vertex;
00295     vertex.x = convex_hull.points[i].x;
00296     vertex.y = convex_hull.points[i].y;
00297     if (table_padding_ > 0.0)
00298     {
00299       double dx = vertex.x - centroid.x;
00300       double dy = vertex.y - centroid.y;
00301       double l = sqrt(dx*dx + dy*dy);
00302       dx /= l; dy /= l;
00303       vertex.x += table_padding_ * dx;
00304       vertex.y += table_padding_ * dy;
00305     }
00306     if(flatten_table) vertex.z = 0;
00307     else vertex.z = convex_hull.points[i].z;
00308     table.convex_hull.vertices.push_back(vertex);
00309       
00310     if(i==0 || i==convex_hull.points.size()-1) continue;
00311     shape_msgs::MeshTriangle meshtri;
00312     meshtri.vertex_indices[0] = 0;
00313     meshtri.vertex_indices[1] = i;
00314     meshtri.vertex_indices[2] = i+1;
00315     table.convex_hull.triangles.push_back(meshtri);
00316   }
00317   visualization_msgs::Marker tableMarker = MarkerGenerator::getConvexHullTableMarker(table.convex_hull);
00318   tableMarker.header = table.pose.header;
00319   tableMarker.pose = table.pose.pose;
00320   tableMarker.ns = "tabletop_node";
00321   tableMarker.id = current_marker_id_++;
00322   marker_pub_.publish(tableMarker);
00323 
00324   visualization_msgs::Marker originMarker = 
00325     MarkerGenerator::createMarker(table.pose.header.frame_id, 0, .0025, .0025, .01, 0, 1, 1, 
00326                                   visualization_msgs::Marker::CUBE, current_marker_id_++, 
00327                                   "tabletop_node", table.pose.pose);
00328   marker_pub_.publish(originMarker);
00329 }
00330 
00331 template <class PointCloudType>
00332 Table TabletopSegmentor::getTable(std_msgs::Header cloud_header,
00333                                   const tf::Transform &table_plane_trans, 
00334                                   const PointCloudType &table_points)
00335 {
00336   Table table;
00337  
00338   //get the extents of the table
00339   if (!table_points.points.empty()) 
00340   {
00341     table.x_min = table_points.points[0].x;
00342     table.x_max = table_points.points[0].x;
00343     table.y_min = table_points.points[0].y;
00344     table.y_max = table_points.points[0].y;
00345   }  
00346   for (size_t i=1; i<table_points.points.size(); ++i) 
00347   {
00348     if (table_points.points[i].x<table.x_min && table_points.points[i].x>-3.0) table.x_min = table_points.points[i].x;
00349     if (table_points.points[i].x>table.x_max && table_points.points[i].x< 3.0) table.x_max = table_points.points[i].x;
00350     if (table_points.points[i].y<table.y_min && table_points.points[i].y>-3.0) table.y_min = table_points.points[i].y;
00351     if (table_points.points[i].y>table.y_max && table_points.points[i].y< 3.0) table.y_max = table_points.points[i].y;
00352   }
00353 
00354   geometry_msgs::Pose table_pose;
00355   tf::poseTFToMsg(table_plane_trans, table_pose);
00356   table.pose.pose = table_pose;
00357   table.pose.header = cloud_header;
00358 
00359   
00360   visualization_msgs::Marker tableMarker = MarkerGenerator::getTableMarker(table.x_min, table.x_max,
00361                                                                            table.y_min, table.y_max);
00362   tableMarker.header = cloud_header;
00363   tableMarker.pose = table_pose;
00364   tableMarker.ns = "tabletop_node";
00365   tableMarker.id = current_marker_id_++;
00366   marker_pub_.publish(tableMarker);
00367   
00368   return table;
00369 }
00370 
00371 template <class PointCloudType>
00372 void TabletopSegmentor::publishClusterMarkers(const std::vector<PointCloudType> &clusters, std_msgs::Header cloud_header)
00373 {
00374   for (size_t i=0; i<clusters.size(); i++) 
00375   {
00376     visualization_msgs::Marker cloud_marker =  MarkerGenerator::getCloudMarker(clusters[i]);
00377     cloud_marker.header = cloud_header;
00378     cloud_marker.pose.orientation.w = 1;
00379     cloud_marker.ns = "tabletop_node";
00380     cloud_marker.id = current_marker_id_++;
00381     marker_pub_.publish(cloud_marker);
00382   }
00383 }
00384 
00385 void TabletopSegmentor::clearOldMarkers(std::string frame_id)
00386 {
00387   for (int id=current_marker_id_; id < num_markers_published_; id++)
00388     {
00389       visualization_msgs::Marker delete_marker;
00390       delete_marker.header.stamp = ros::Time::now();
00391       delete_marker.header.frame_id = frame_id;
00392       delete_marker.id = id;
00393       delete_marker.action = visualization_msgs::Marker::DELETE;
00394       delete_marker.ns = "tabletop_node";
00395       marker_pub_.publish(delete_marker);
00396     }
00397   num_markers_published_ = current_marker_id_;
00398   current_marker_id_ = 0;
00399 }
00400 
00402 tf::Transform getPlaneTransform (pcl::ModelCoefficients coeffs, double up_direction, bool flatten_plane)
00403 {
00404   ROS_ASSERT(coeffs.values.size() > 3);
00405   double a = coeffs.values[0], b = coeffs.values[1], c = coeffs.values[2], d = coeffs.values[3];
00406   //asume plane coefficients are normalized
00407   tf::Vector3 position(-a*d, -b*d, -c*d);
00408   tf::Vector3 z(a, b, c);
00409 
00410   //if we are flattening the plane, make z just be (0,0,up_direction)
00411   if(flatten_plane)
00412   {
00413     ROS_INFO("flattening plane");
00414     z[0] = z[1] = 0;
00415     z[2] = up_direction;
00416   }
00417   else
00418   {
00419     //make sure z points "up"
00420     ROS_DEBUG("in getPlaneTransform, z: %0.3f, %0.3f, %0.3f", z[0], z[1], z[2]);
00421     if ( z.dot( tf::Vector3(0, 0, up_direction) ) < 0)
00422     {
00423       z = -1.0 * z;
00424       ROS_INFO("flipped z");
00425     }
00426   }
00427     
00428   //try to align the x axis with the x axis of the original frame
00429   //or the y axis if z and x are too close too each other
00430   tf::Vector3 x(1, 0, 0);
00431   if ( fabs(z.dot(x)) > 1.0 - 1.0e-4) x = tf::Vector3(0, 1, 0);
00432   tf::Vector3 y = z.cross(x).normalized();
00433   x = y.cross(z).normalized();
00434 
00435   tf::Matrix3x3 rotation;
00436   rotation[0] = x;      // x
00437   rotation[1] = y;      // y
00438   rotation[2] = z;      // z
00439   rotation = rotation.transpose();
00440   tf::Quaternion orientation;
00441   rotation.getRotation(orientation);
00442   ROS_DEBUG("in getPlaneTransform, x: %0.3f, %0.3f, %0.3f", x[0], x[1], x[2]);
00443   ROS_DEBUG("in getPlaneTransform, y: %0.3f, %0.3f, %0.3f", y[0], y[1], y[2]);
00444   ROS_DEBUG("in getPlaneTransform, z: %0.3f, %0.3f, %0.3f", z[0], z[1], z[2]);
00445   return tf::Transform(orientation, position);
00446 }
00447 
00448 template <class PointCloudType>
00449 bool TabletopSegmentor::tableMsgToPointCloud (Table &table, std::string frame_id, PointCloudType &table_hull)
00450 {
00451   //use table.pose (PoseStamped) to transform table.convex_hull.vertices (Point[]) into a pcl::PointCloud in frame_id
00452   ros::Time now = ros::Time::now();
00453   PointCloudType table_frame_points;
00454   table_frame_points.header.stamp = now;
00455   table_frame_points.header.frame_id = "table_frame";
00456   table_frame_points.points.resize(table.convex_hull.vertices.size());
00457   for(size_t i=0; i < table.convex_hull.vertices.size(); i++)
00458   {
00459     table_frame_points.points[i].x = table.convex_hull.vertices[i].x;
00460     table_frame_points.points[i].y = table.convex_hull.vertices[i].y;
00461     table_frame_points.points[i].z = table.convex_hull.vertices[i].z;
00462   }
00463 
00464   //add the table frame transform to the tf listener
00465   tf::Transform table_trans;
00466   tf::poseMsgToTF(table.pose.pose, table_trans);
00467   tf::StampedTransform table_pose_frame(table_trans, now, table.pose.header.frame_id, "table_frame");
00468   listener_.setTransform(table_pose_frame);
00469   ROS_INFO("transforming point cloud from table frame to frame %s", frame_id.c_str());
00470 
00471   //make sure we can transform
00472   std::string error_msg;
00473   if (!listener_.waitForTransform(frame_id, "table_frame", now, ros::Duration(2.0), ros::Duration(0.01), &error_msg))
00474   {
00475     ROS_ERROR("Can not transform point cloud from table frame to frame %s; error %s", 
00476               frame_id.c_str(), error_msg.c_str());
00477     return false;
00478   }
00479 
00480   //transform the points
00481   int current_try=0, max_tries = 3;
00482   while (1)
00483   {
00484     bool transform_success = true;
00485     try
00486     {
00487       pcl_ros::transformPointCloud<Point>(frame_id, table_frame_points, table_hull, listener_); 
00488     }
00489     catch (tf::TransformException ex)
00490     {
00491       transform_success = false;
00492       if ( ++current_try >= max_tries )
00493       {
00494         ROS_ERROR("Failed to transform point cloud from table frame into frame %s; error %s", 
00495                   frame_id.c_str(), ex.what());
00496         return false;
00497       }
00498       //sleep a bit to give the listener a chance to get a new transform
00499       ros::Duration(0.1).sleep();
00500     }
00501     if (transform_success) break;
00502   }
00503   table_hull.header.frame_id = frame_id;
00504   table_hull.header.stamp = now;
00505 
00506   //copy the transformed points back into the Table message and set the pose to identity in the cloud frame
00507   for(size_t i=0; i < table.convex_hull.vertices.size(); i++)
00508   {
00509     table.convex_hull.vertices[i].x = table_hull.points[i].x;
00510     table.convex_hull.vertices[i].y = table_hull.points[i].y;
00511     table.convex_hull.vertices[i].z = table_hull.points[i].z;
00512   }
00513   geometry_msgs::PoseStamped iden;
00514   iden.pose.orientation.w = 1;
00515   table.pose = iden;
00516   table.pose.header.frame_id = frame_id;
00517 
00518   //make a new Shape for drawing
00519   shape_msgs::Mesh mesh;
00520   mesh.vertices.resize(table_hull.points.size());
00521   for(size_t i = 0; i < table_hull.points.size(); i++)
00522   {
00523     mesh.vertices[i].x = table_hull.points[i].x;
00524     mesh.vertices[i].y = table_hull.points[i].y;
00525     mesh.vertices[i].z = table_hull.points[i].z;
00526   }
00527   mesh.triangles = table.convex_hull.triangles;
00528   visualization_msgs::Marker tableMarker = MarkerGenerator::getConvexHullTableMarker(mesh);
00529   tableMarker.header = table_hull.header;
00530   tableMarker.pose.orientation.w = 1.0;
00531   tableMarker.ns = "tabletop_node";
00532   tableMarker.id = current_marker_id_++;
00533   marker_pub_.publish(tableMarker);
00534 
00535   return true;
00536 }
00537 
00538 
00539 template <typename PointT> 
00540 bool getPlanePoints (const pcl::PointCloud<PointT> &table, 
00541                      const tf::Transform& table_plane_trans,
00542                      sensor_msgs::PointCloud &table_points)
00543 {
00544   // Prepare the output
00545   table_points.header = table.header;
00546   table_points.points.resize (table.points.size ());
00547   for (size_t i = 0; i < table.points.size (); ++i)
00548   {
00549     table_points.points[i].x = table.points[i].x;
00550     table_points.points[i].y = table.points[i].y;
00551     table_points.points[i].z = table.points[i].z;
00552   }
00553 
00554   // Transform the data
00555   tf::TransformListener listener;
00556   tf::StampedTransform table_pose_frame(table_plane_trans, table.header.stamp, 
00557                                         table.header.frame_id, "table_frame");
00558   listener.setTransform(table_pose_frame);
00559   std::string error_msg;
00560   if (!listener.canTransform("table_frame", table_points.header.frame_id, table_points.header.stamp, &error_msg))
00561   {
00562     ROS_ERROR("Can not transform point cloud from frame %s to table frame; error %s", 
00563               table_points.header.frame_id.c_str(), error_msg.c_str());
00564     return false;
00565   }
00566   int current_try=0, max_tries = 3;
00567   while (1)
00568   {
00569     bool transform_success = true;
00570     try
00571     {
00572       listener.transformPointCloud("table_frame", table_points, table_points);
00573     }
00574     catch (tf::TransformException ex)
00575     {
00576       transform_success = false;
00577       if ( ++current_try >= max_tries )
00578       {
00579         ROS_ERROR("Failed to transform point cloud from frame %s into table_frame; error %s", 
00580                   table_points.header.frame_id.c_str(), ex.what());
00581         return false;
00582       }
00583       //sleep a bit to give the listener a chance to get a new transform
00584       ros::Duration(0.1).sleep();
00585     }
00586     if (transform_success) break;
00587   }
00588   table_points.header.stamp = table.header.stamp;
00589   table_points.header.frame_id = "table_frame";
00590   return true;
00591 }
00592 
00593  
00594 template <class PointCloudType>
00595 void straightenPoints(PointCloudType &points, const tf::Transform& table_plane_trans, 
00596                       const tf::Transform& table_plane_trans_flat)
00597 {
00598   tf::Transform trans = table_plane_trans_flat * table_plane_trans.inverse();
00599   pcl_ros::transformPointCloud(points, points, trans);
00600 }
00601 
00602 
00603 template <typename PointT> void
00604 getClustersFromPointCloud2 (const pcl::PointCloud<PointT> &cloud_objects,                           
00605                             const std::vector<pcl::PointIndices> &clusters2, 
00606                             std::vector<sensor_msgs::PointCloud> &clusters)
00607 {
00608   clusters.resize (clusters2.size ());
00609   for (size_t i = 0; i < clusters2.size (); ++i)
00610   {
00611     pcl::PointCloud<PointT> cloud_cluster;
00612     pcl::copyPointCloud(cloud_objects, clusters2[i], cloud_cluster);
00613     sensor_msgs::PointCloud2 pc2;
00614     pcl::toROSMsg( cloud_cluster, pc2 ); 
00615     sensor_msgs::convertPointCloud2ToPointCloud (pc2, clusters[i]);    
00616   }
00617 }
00618 
00619 void TabletopSegmentor::processCloud(const sensor_msgs::PointCloud2 &cloud,
00620                                      TabletopSegmentation::Response &response,
00621                                      Table input_table)
00622 {
00623   ROS_INFO("Starting process on new cloud in frame %s", cloud.header.frame_id.c_str());
00624 
00625   // PCL objects
00626   KdTreePtr normals_tree_, clusters_tree_;
00627   pcl::VoxelGrid<Point> grid_, grid_objects_;
00628   pcl::PassThrough<Point> pass_;
00629   pcl::NormalEstimation<Point, pcl::Normal> n3d_;
00630   pcl::SACSegmentationFromNormals<Point, pcl::Normal> seg_;
00631   pcl::ProjectInliers<Point> proj_;
00632   pcl::ConvexHull<Point> hull_;
00633   pcl::ExtractPolygonalPrismData<Point> prism_;
00634   pcl::EuclideanClusterExtraction<Point> pcl_cluster_;
00635   pcl::PointCloud<Point>::Ptr table_hull_ptr (new pcl::PointCloud<Point>); 
00636 
00637   // Filtering parameters
00638   grid_.setLeafSize (plane_detection_voxel_size_, plane_detection_voxel_size_, plane_detection_voxel_size_);
00639   grid_objects_.setLeafSize (clustering_voxel_size_, clustering_voxel_size_, clustering_voxel_size_);
00640   grid_.setFilterFieldName ("z");
00641   grid_.setFilterLimits (z_filter_min_, z_filter_max_);
00642   grid_.setDownsampleAllData (false);
00643   grid_objects_.setDownsampleAllData (true);
00644 
00645   normals_tree_ = boost::make_shared<pcl::search::KdTree<Point> > ();
00646   clusters_tree_ = boost::make_shared<pcl::search::KdTree<Point> > ();
00647 
00648   // Normal estimation parameters
00649   n3d_.setKSearch (10);  
00650   n3d_.setSearchMethod (normals_tree_);
00651   // Table model fitting parameters
00652   seg_.setDistanceThreshold (0.05); 
00653   seg_.setMaxIterations (10000);
00654   seg_.setNormalDistanceWeight (0.1);
00655   seg_.setOptimizeCoefficients (true);
00656   seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
00657   seg_.setMethodType (pcl::SAC_RANSAC);
00658   seg_.setProbability (0.99);
00659 
00660   proj_.setModelType (pcl::SACMODEL_PLANE);
00661 
00662   // Clustering parameters
00663   pcl_cluster_.setClusterTolerance (cluster_distance_);
00664   pcl_cluster_.setMinClusterSize (min_cluster_size_);
00665   pcl_cluster_.setSearchMethod (clusters_tree_);
00666 
00667   // Step 1 : Filter, remove NaNs and downsample
00668   pcl::PointCloud<Point>::Ptr cloud_ptr (new pcl::PointCloud<Point>); 
00669   pcl::fromROSMsg (cloud, *cloud_ptr);
00670   pass_.setInputCloud (cloud_ptr);
00671   pass_.setFilterFieldName ("z");
00672   pass_.setFilterLimits (z_filter_min_, z_filter_max_);
00673   pcl::PointCloud<Point>::Ptr z_cloud_filtered_ptr (new pcl::PointCloud<Point>); 
00674   pass_.filter (*z_cloud_filtered_ptr);
00675 
00676   pass_.setInputCloud (z_cloud_filtered_ptr);
00677   pass_.setFilterFieldName ("y");
00678   pass_.setFilterLimits (y_filter_min_, y_filter_max_);
00679   pcl::PointCloud<Point>::Ptr y_cloud_filtered_ptr (new pcl::PointCloud<Point>); 
00680   pass_.filter (*y_cloud_filtered_ptr);
00681 
00682   pass_.setInputCloud (y_cloud_filtered_ptr);
00683   pass_.setFilterFieldName ("x");
00684   pass_.setFilterLimits (x_filter_min_, x_filter_max_);
00685   pcl::PointCloud<Point>::Ptr cloud_filtered_ptr (new pcl::PointCloud<Point>); 
00686   pass_.filter (*cloud_filtered_ptr);
00687   
00688   ROS_INFO("Step 1 done");
00689   if (cloud_filtered_ptr->points.size() < (unsigned int)min_cluster_size_)
00690   {
00691     ROS_INFO("Filtered cloud only has %d points", (int)cloud_filtered_ptr->points.size());
00692     response.result = response.NO_TABLE;
00693     return;
00694   }
00695 
00696   pcl::PointCloud<Point>::Ptr cloud_downsampled_ptr (new pcl::PointCloud<Point>); 
00697   grid_.setInputCloud (cloud_filtered_ptr);
00698   grid_.filter (*cloud_downsampled_ptr);
00699   if (cloud_downsampled_ptr->points.size() < (unsigned int)min_cluster_size_)
00700   {
00701     ROS_INFO("Downsampled cloud only has %d points", (int)cloud_downsampled_ptr->points.size());
00702     response.result = response.NO_TABLE;    
00703     return;
00704   }
00705 
00706   // Step 2 : Estimate normals
00707   pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ptr (new pcl::PointCloud<pcl::Normal>); 
00708   n3d_.setInputCloud (cloud_downsampled_ptr);
00709   n3d_.compute (*cloud_normals_ptr);
00710   ROS_INFO("Step 2 done");
00711 
00712 
00713   // Step 3 : Perform planar segmentation, if table is not given, otherwise use given table
00714   tf::Transform table_plane_trans; 
00715   tf::Transform table_plane_trans_flat;
00716   if(input_table.convex_hull.vertices.size() != 0)
00717   {  
00718     ROS_INFO("Table input, skipping Step 3");
00719     bool success = tableMsgToPointCloud<pcl::PointCloud<Point> >(input_table, cloud.header.frame_id, *table_hull_ptr);
00720     if(!success)
00721     {
00722       ROS_ERROR("Failure in converting table convex hull!");
00723       return;
00724     }
00725     response.table = input_table;
00726     if(flatten_table_)
00727     {
00728       ROS_ERROR("flatten_table mode is disabled if table is given!");
00729       flatten_table_ = false;
00730     }
00731   }
00732   else
00733   {
00734     pcl::PointIndices::Ptr table_inliers_ptr (new pcl::PointIndices); 
00735     pcl::ModelCoefficients::Ptr table_coefficients_ptr (new pcl::ModelCoefficients); 
00736     seg_.setInputCloud (cloud_downsampled_ptr);
00737     seg_.setInputNormals (cloud_normals_ptr);
00738     seg_.segment (*table_inliers_ptr, *table_coefficients_ptr);
00739  
00740     if (table_coefficients_ptr->values.size () <=3)
00741     {
00742       ROS_INFO("Failed to detect table in scan");
00743       response.result = response.NO_TABLE;    
00744       return;
00745     }
00746 
00747     if ( table_inliers_ptr->indices.size() < (unsigned int)inlier_threshold_)
00748     {
00749       ROS_INFO("Plane detection has %d inliers, below min threshold of %d", (int)table_inliers_ptr->indices.size(),
00750                inlier_threshold_);
00751       response.result = response.NO_TABLE;
00752       return;
00753     }
00754 
00755     ROS_INFO ("[TableObjectDetector::input_callback] Model found with %d inliers: [%f %f %f %f].", 
00756               (int)table_inliers_ptr->indices.size (),
00757               table_coefficients_ptr->values[0], table_coefficients_ptr->values[1], 
00758               table_coefficients_ptr->values[2], table_coefficients_ptr->values[3]);
00759     ROS_INFO("Step 3 done");
00760 
00761     // Step 4 : Project the table inliers on the table
00762     pcl::PointCloud<Point>::Ptr table_projected_ptr (new pcl::PointCloud<Point>); 
00763     proj_.setInputCloud (cloud_downsampled_ptr);
00764     proj_.setIndices (table_inliers_ptr);
00765     proj_.setModelCoefficients (table_coefficients_ptr);
00766     proj_.filter (*table_projected_ptr);
00767     ROS_INFO("Step 4 done");
00768   
00769     sensor_msgs::PointCloud table_points;
00770     sensor_msgs::PointCloud table_hull_points;
00771     table_plane_trans = getPlaneTransform (*table_coefficients_ptr, up_direction_, false);
00772 
00773     // ---[ Estimate the convex hull (not in table frame)
00774     hull_.setInputCloud (table_projected_ptr);
00775     hull_.reconstruct (*table_hull_ptr);
00776 
00777     if(!flatten_table_)
00778     {
00779       // --- [ Take the points projected on the table and transform them into the PointCloud message
00780       //  while also transforming them into the table's coordinate system
00781       if (!getPlanePoints<Point> (*table_projected_ptr, table_plane_trans, table_points))
00782       {
00783         response.result = response.OTHER_ERROR;
00784         return;
00785       }
00786 
00787       // ---[ Create the table message
00788       response.table = getTable<sensor_msgs::PointCloud>(cloud.header, table_plane_trans, table_points);
00789 
00790       // ---[ Convert the convex hull points to table frame
00791       if (!getPlanePoints<Point> (*table_hull_ptr, table_plane_trans, table_hull_points))
00792       {
00793         response.result = response.OTHER_ERROR;
00794         return;
00795       }
00796     }
00797     if(flatten_table_)
00798     {
00799       // if flattening the table, find the center of the convex hull and move the table frame there
00800       table_plane_trans_flat = getPlaneTransform (*table_coefficients_ptr, up_direction_, flatten_table_);
00801       tf::Vector3 flat_table_pos;
00802       double avg_x, avg_y, avg_z;
00803       avg_x = avg_y = avg_z = 0;
00804       for (size_t i=0; i<table_projected_ptr->points.size(); i++)
00805       {
00806         avg_x += table_projected_ptr->points[i].x;
00807         avg_y += table_projected_ptr->points[i].y;
00808         avg_z += table_projected_ptr->points[i].z;
00809       }
00810       avg_x /= table_projected_ptr->points.size();
00811       avg_y /= table_projected_ptr->points.size();
00812       avg_z /= table_projected_ptr->points.size();
00813       ROS_INFO("average x,y,z = (%.5f, %.5f, %.5f)", avg_x, avg_y, avg_z);
00814 
00815       // place the new table frame in the center of the convex hull
00816       flat_table_pos[0] = avg_x;
00817       flat_table_pos[1] = avg_y;
00818       flat_table_pos[2] = avg_z;
00819       table_plane_trans_flat.setOrigin(flat_table_pos);
00820 
00821       // shift the non-flat table frame to the center of the convex hull as well
00822       table_plane_trans.setOrigin(flat_table_pos);
00823 
00824       // --- [ Take the points projected on the table and transform them into the PointCloud message
00825       //  while also transforming them into the flat table's coordinate system
00826       sensor_msgs::PointCloud flat_table_points;
00827       if (!getPlanePoints<Point> (*table_projected_ptr, table_plane_trans_flat, flat_table_points))
00828       {
00829         response.result = response.OTHER_ERROR;
00830         return;
00831       }
00832 
00833       // ---[ Create the table message
00834       response.table = getTable<sensor_msgs::PointCloud>(cloud.header, table_plane_trans_flat, flat_table_points);
00835 
00836       // ---[ Convert the convex hull points to flat table frame
00837       if (!getPlanePoints<Point> (*table_hull_ptr, table_plane_trans_flat, table_hull_points))
00838       {
00839         response.result = response.OTHER_ERROR;
00840         return;
00841       }
00842     }
00843 
00844     ROS_INFO("Table computed");  
00845     // ---[ Add the convex hull as a triangle mesh to the Table message
00846     addConvexHullTable<sensor_msgs::PointCloud>(response.table, table_hull_points, flatten_table_);
00847   }
00848 
00849   // Step 5: Get the objects on top of the (non-flat) table
00850   pcl::PointIndices cloud_object_indices;
00851   //prism_.setInputCloud (cloud_all_minus_table_ptr);
00852   prism_.setInputCloud (cloud_filtered_ptr);
00853   prism_.setInputPlanarHull (table_hull_ptr);
00854   ROS_INFO("Using table prism: %f to %f", table_z_filter_min_, table_z_filter_max_);
00855   prism_.setHeightLimits (table_z_filter_min_, table_z_filter_max_);  
00856   prism_.segment (cloud_object_indices);
00857 
00858   pcl::PointCloud<Point>::Ptr cloud_objects_ptr (new pcl::PointCloud<Point>); 
00859   pcl::ExtractIndices<Point> extract_object_indices;
00860   extract_object_indices.setInputCloud (cloud_filtered_ptr);
00861   extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
00862   extract_object_indices.filter (*cloud_objects_ptr);
00863 
00864   ROS_INFO (" Number of object point candidates: %d.", (int)cloud_objects_ptr->points.size ());
00865   response.result = response.SUCCESS;
00866 
00867   if (cloud_objects_ptr->points.empty ()) 
00868   {
00869     ROS_INFO("No objects on table");
00870     return;
00871   }
00872 
00873   // ---[ Downsample the points
00874   pcl::PointCloud<Point>::Ptr cloud_objects_downsampled_ptr (new pcl::PointCloud<Point>); 
00875   grid_objects_.setInputCloud (cloud_objects_ptr);
00876   grid_objects_.filter (*cloud_objects_downsampled_ptr);
00877 
00878   // ---[ If flattening the table, adjust the points on the table to be straight also
00879   if(flatten_table_) straightenPoints<pcl::PointCloud<Point> >(*cloud_objects_downsampled_ptr, 
00880                                                                table_plane_trans, table_plane_trans_flat);
00881 
00882   // Step 6: Split the objects into Euclidean clusters
00883   std::vector<pcl::PointIndices> clusters2;
00884   //pcl_cluster_.setInputCloud (cloud_objects_ptr);
00885   pcl_cluster_.setInputCloud (cloud_objects_downsampled_ptr);
00886   pcl_cluster_.extract (clusters2);
00887   ROS_INFO ("Number of clusters found matching the given constraints: %d.", (int)clusters2.size ());
00888 
00889   // ---[ Convert clusters into the PointCloud message
00890   std::vector<sensor_msgs::PointCloud> clusters;
00891   getClustersFromPointCloud2<Point> (*cloud_objects_downsampled_ptr, clusters2, clusters);
00892   ROS_INFO("Clusters converted");
00893   response.clusters = clusters;  
00894 
00895   publishClusterMarkers(clusters, cloud.header);
00896 }
00897 
00898 
00899 } //namespace tabletop_object_detector
00900 
00901 int main(int argc, char **argv) 
00902 {
00903   ros::init(argc, argv, "tabletop_segmentation_node");
00904   ros::NodeHandle nh;
00905 
00906   tabletop_object_detector::TabletopSegmentor node(nh);
00907   ros::spin();
00908   return 0;
00909 }


tabletop_object_detector
Author(s): Marius Muja and Matei Ciocarlie
autogenerated on Mon Oct 6 2014 11:45:33