bosch_object_segmentation_gui: bosch_object_segmentation_gui: table

00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2011, Robert Bosch LLC.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Robert Bosch nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  *********************************************************************/
00036 
00037 #include "bosch_object_segmentation_gui/table_detector.h"
00038 
00039 #include <pcl/ros/conversions.h>
00040 #include <pcl/point_types.h>
00041 #include <pcl_ros/transforms.h>
00042 #include <pcl/point_cloud.h>
00043 #include <pcl/point_types.h>
00044 #include <pcl/io/io.h>
00045 #include <pcl/filters/voxel_grid.h>
00046 #include <pcl/filters/passthrough.h>
00047 #include <pcl/filters/extract_indices.h>
00048 #include <pcl/features/normal_3d.h>
00049 #include <pcl/kdtree/kdtree_flann.h>
00050 #include <pcl/sample_consensus/method_types.h>
00051 #include <pcl/sample_consensus/model_types.h>
00052 #include <pcl/segmentation/sac_segmentation.h>
00053 #include <pcl/filters/project_inliers.h>
00054 #include <pcl/surface/convex_hull.h>
00055 #include <pcl/segmentation/extract_polygonal_prism_data.h>
00056 #include <pcl/segmentation/extract_clusters.h>
00057 
00058 namespace bosch_object_segmentation_gui {
00059   
00060 TableDetector::TableDetector() : current_marker_id_(0)
00061 {
00062   //initialize operational flags
00063   inlier_threshold_ = 300;
00064   plane_detection_voxel_size_ = 0.01;
00065   clustering_voxel_size_ = 0.003;
00066   z_filter_min_ = 0.4;
00067   z_filter_max_ = 1.25;
00068   table_z_filter_min_ = 0.01;
00069   table_z_filter_max_ = 0.50;
00070   cluster_distance_ = 0.03;
00071   min_cluster_size_ = 30;
00072   processing_frame_ = "";
00073   up_direction_ = -1.0;
00074 }
00075 
00076 TableDetector::~TableDetector()
00077 {
00078 }
00079 
00080 bool TableDetector::detectTable(const sensor_msgs::PointCloud2 &cloud, tabletop_object_detector::Table& table)
00081 {
00082   ROS_INFO("Starting process on new cloud");
00083   ROS_INFO("In frame %s", cloud.header.frame_id.c_str());
00084 
00085   // PCL objects
00086   KdTreePtr normals_tree_, clusters_tree_;
00087   pcl::VoxelGrid<Point> grid_, grid_objects_;
00088   pcl::PassThrough<Point> pass_;
00089   pcl::NormalEstimation<Point, pcl::Normal> n3d_;
00090   pcl::SACSegmentationFromNormals<Point, pcl::Normal> seg_;
00091   pcl::ProjectInliers<Point> proj_;
00092   pcl::ConvexHull<Point> hull_;
00093   pcl::ExtractPolygonalPrismData<Point> prism_;
00094   pcl::EuclideanClusterExtraction<Point> pcl_cluster_;
00095 
00096   // Filtering parameters
00097   grid_.setLeafSize (plane_detection_voxel_size_, plane_detection_voxel_size_, plane_detection_voxel_size_);
00098   grid_objects_.setLeafSize (clustering_voxel_size_, clustering_voxel_size_, clustering_voxel_size_);
00099   grid_.setFilterFieldName ("z");
00100   pass_.setFilterFieldName ("z");
00101 
00102   pass_.setFilterLimits (z_filter_min_, z_filter_max_);
00103   grid_.setFilterLimits (z_filter_min_, z_filter_max_);
00104   grid_.setDownsampleAllData (false);
00105   grid_objects_.setDownsampleAllData (false);
00106 
00107   normals_tree_ = boost::make_shared<pcl::KdTreeFLANN<Point> > ();
00108   clusters_tree_ = boost::make_shared<pcl::KdTreeFLANN<Point> > ();
00109 
00110   // Normal estimation parameters
00111   n3d_.setKSearch (10);
00112   n3d_.setSearchMethod (normals_tree_);
00113   // Table model fitting parameters
00114   seg_.setDistanceThreshold (0.05);
00115   seg_.setMaxIterations (10000);
00116   seg_.setNormalDistanceWeight (0.1);
00117   seg_.setOptimizeCoefficients (true);
00118   seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
00119   seg_.setMethodType (pcl::SAC_RANSAC);
00120   seg_.setProbability (0.99);
00121 
00122   proj_.setModelType (pcl::SACMODEL_PLANE);
00123 
00124   // Consider only objects in a given layer above the table
00125   prism_.setHeightLimits (table_z_filter_min_, table_z_filter_max_);
00126 
00127   // Clustering parameters
00128   pcl_cluster_.setClusterTolerance (cluster_distance_);
00129   pcl_cluster_.setMinClusterSize (min_cluster_size_);
00130   pcl_cluster_.setSearchMethod (clusters_tree_);
00131 
00132   // Step 1 : Filter, remove NaNs and downsample
00133   pcl::PointCloud<Point> cloud_t;
00134   pcl::fromROSMsg (cloud, cloud_t);
00135   pcl::PointCloud<Point>::ConstPtr cloud_ptr = boost::make_shared<const pcl::PointCloud<Point> > (cloud_t);
00136 
00137   pcl::PointCloud<Point> cloud_filtered;
00138   pass_.setInputCloud (cloud_ptr);
00139   pass_.filter (cloud_filtered);
00140   pcl::PointCloud<Point>::ConstPtr cloud_filtered_ptr =
00141     boost::make_shared<const pcl::PointCloud<Point> > (cloud_filtered);
00142   ROS_INFO("Step 1 done");
00143   if (cloud_filtered.points.size() < (unsigned int)min_cluster_size_)
00144   {
00145     ROS_INFO("Filtered cloud only has %d points", (int)cloud_filtered.points.size());
00146     return false;
00147   }
00148 
00149   pcl::PointCloud<Point> cloud_downsampled;
00150   grid_.setInputCloud (cloud_filtered_ptr);
00151   grid_.filter (cloud_downsampled);
00152   pcl::PointCloud<Point>::ConstPtr cloud_downsampled_ptr =
00153     boost::make_shared<const pcl::PointCloud<Point> > (cloud_downsampled);
00154   if (cloud_downsampled.points.size() < (unsigned int)min_cluster_size_)
00155   {
00156     ROS_INFO("Downsampled cloud only has %d points", (int)cloud_downsampled.points.size());
00157     return false;
00158   }
00159 
00160   // Step 2 : Estimate normals
00161   pcl::PointCloud<pcl::Normal> cloud_normals;
00162   n3d_.setInputCloud (cloud_downsampled_ptr);
00163   n3d_.compute (cloud_normals);
00164   pcl::PointCloud<pcl::Normal>::ConstPtr cloud_normals_ptr =
00165     boost::make_shared<const pcl::PointCloud<pcl::Normal> > (cloud_normals);
00166   ROS_INFO("Step 2 done");
00167 
00168   // Step 3 : Perform planar segmentation
00169   pcl::PointIndices table_inliers; pcl::ModelCoefficients table_coefficients;
00170   seg_.setInputCloud (cloud_downsampled_ptr);
00171   seg_.setInputNormals (cloud_normals_ptr);
00172   seg_.segment (table_inliers, table_coefficients);
00173   pcl::PointIndices::ConstPtr table_inliers_ptr = boost::make_shared<const pcl::PointIndices> (table_inliers);
00174   pcl::ModelCoefficients::ConstPtr table_coefficients_ptr =
00175     boost::make_shared<const pcl::ModelCoefficients> (table_coefficients);
00176 
00177   if (table_coefficients.values.size () <=3)
00178   {
00179     ROS_INFO("Failed to detect table in scan");
00180     return false;
00181   }
00182 
00183   if ( table_inliers.indices.size() < (unsigned int)inlier_threshold_)
00184   {
00185     ROS_INFO("Plane detection has %d inliers, below min threshold of %d", (int)table_inliers.indices.size(),
00186        inlier_threshold_);
00187     return false;
00188   }
00189 
00190   ROS_INFO ("[TableObjectDetector::input_callback] Model found with %d inliers: [%f %f %f %f].",
00191       (int)table_inliers.indices.size (),
00192       table_coefficients.values[0], table_coefficients.values[1],
00193       table_coefficients.values[2], table_coefficients.values[3]);
00194   ROS_INFO("Step 3 done");
00195 
00196   // Step 4 : Project the table inliers on the table
00197   pcl::PointCloud<Point> table_projected;
00198   proj_.setInputCloud (cloud_downsampled_ptr);
00199   proj_.setIndices (table_inliers_ptr);
00200   proj_.setModelCoefficients (table_coefficients_ptr);
00201   proj_.filter (table_projected);
00202   pcl::PointCloud<Point>::ConstPtr table_projected_ptr =
00203     boost::make_shared<const pcl::PointCloud<Point> > (table_projected);
00204   ROS_INFO("Step 4 done");
00205 
00206   sensor_msgs::PointCloud table_points;
00207   tf::Transform table_plane_trans = getPlaneTransform (table_coefficients, up_direction_);
00208   //takes the points projected on the table and transforms them into the PointCloud message
00209   //while also transforming them into the table's coordinate system
00210   if (!getPlanePoints(table_projected, table_plane_trans, table_points))
00211   {
00212     return false;
00213   }
00214   ROS_INFO("Table computed");
00215 
00216   table = getTable(cloud.header, table_plane_trans, table_points);
00217   return true;
00218 }
00219 
00220 tabletop_object_detector::Table
00221 TableDetector::getTable(const std_msgs::Header& cloud_header, const tf::Transform &table_plane_trans, const sensor_msgs::PointCloud &table_points)
00222 {
00223   tabletop_object_detector::Table table;
00224 
00225   //get the extents of the table
00226   if (!table_points.points.empty())
00227   {
00228     table.x_min = table_points.points[0].x;
00229     table.x_max = table_points.points[0].x;
00230     table.y_min = table_points.points[0].y;
00231     table.y_max = table_points.points[0].y;
00232   }
00233   for (size_t i=1; i<table_points.points.size(); ++i)
00234   {
00235     if (table_points.points[i].x<table.x_min && table_points.points[i].x>-3.0) table.x_min = table_points.points[i].x;
00236     if (table_points.points[i].x>table.x_max && table_points.points[i].x< 3.0) table.x_max = table_points.points[i].x;
00237     if (table_points.points[i].y<table.y_min && table_points.points[i].y>-3.0) table.y_min = table_points.points[i].y;
00238     if (table_points.points[i].y>table.y_max && table_points.points[i].y< 3.0) table.y_max = table_points.points[i].y;
00239   }
00240 
00241   geometry_msgs::Pose table_pose;
00242   tf::poseTFToMsg(table_plane_trans, table_pose);
00243   table.pose.pose = table_pose;
00244   table.pose.header.frame_id = cloud_header.frame_id;
00245   table.pose.header.stamp = ros::Time::now();
00246   return table;
00247 }
00248 
00255 visualization_msgs::Marker TableDetector::getTableMarker(const tabletop_object_detector::Table& table)
00256 {
00257   visualization_msgs::Marker marker;
00258   marker.header.frame_id = table.pose.header.frame_id;
00259   marker.header.stamp = ros::Time::now();
00260   marker.pose = table.pose.pose;
00261   marker.ns = "table_detector";
00262   marker.id = current_marker_id_++;
00263 
00264   marker.action = visualization_msgs::Marker::ADD;
00265   marker.type = visualization_msgs::Marker::LINE_STRIP;
00266   marker.lifetime = ros::Duration();
00267 
00268   //create the marker in the table reference frame
00269   //the caller is responsible for setting the pose of the marker to match
00270 
00271   marker.scale.x = 0.001;
00272   marker.scale.y = 0.001;
00273   marker.scale.z = 0.001;
00274 
00275   marker.points.resize(5);
00276   marker.points[0].x = table.x_min;
00277   marker.points[0].y = table.y_min;
00278   marker.points[0].z = 0;
00279 
00280   marker.points[1].x = table.x_min;
00281   marker.points[1].y = table.y_max;
00282   marker.points[1].z = 0;
00283 
00284   marker.points[2].x = table.x_max;
00285   marker.points[2].y = table.y_max;
00286   marker.points[2].z = 0;
00287 
00288   marker.points[3].x = table.x_max;
00289   marker.points[3].y = table.y_min;
00290   marker.points[3].z = 0;
00291 
00292   marker.points[4].x = table.x_min;
00293   marker.points[4].y = table.y_min;
00294   marker.points[4].z = 0;
00295 
00296   marker.points.resize(6);
00297   marker.points[5].x = table.x_min;
00298   marker.points[5].y = table.y_min;
00299   marker.points[5].z = 0.02;
00300 
00301   marker.color.r = 1.0;
00302   marker.color.g = 1.0;
00303   marker.color.b = 0.0;
00304   marker.color.a = 1.0;
00305 
00306   return marker;
00307 }
00308 
00310 tf::Transform TableDetector::getPlaneTransform (pcl::ModelCoefficients coeffs, double up_direction)
00311 {
00312   ROS_ASSERT(coeffs.values.size() > 3);
00313   double a = coeffs.values[0], b = coeffs.values[1], c = coeffs.values[2], d = coeffs.values[3];
00314   //asume plane coefficients are normalized
00315   btVector3 position(-a*d, -b*d, -c*d);
00316   btVector3 z(a, b, c);
00317   //make sure z points "up"
00318   ROS_DEBUG("z.dot: %0.3f", z.dot(btVector3(0,0,1)));
00319   ROS_DEBUG("in getPlaneTransform, z: %0.3f, %0.3f, %0.3f", z[0], z[1], z[2]);
00320   if ( z.dot( btVector3(0, 0, up_direction) ) < 0)
00321   {
00322     z = -1.0 * z;
00323     ROS_INFO("flipped z");
00324   }
00325   ROS_DEBUG("in getPlaneTransform, z: %0.3f, %0.3f, %0.3f", z[0], z[1], z[2]);
00326 
00327   //try to align the x axis with the x axis of the original frame
00328   //or the y axis if z and x are too close too each other
00329   btVector3 x(1, 0, 0);
00330   if ( fabs(z.dot(x)) > 1.0 - 1.0e-4) x = btVector3(0, 1, 0);
00331   btVector3 y = z.cross(x).normalized();
00332   x = y.cross(z).normalized();
00333 
00334   btMatrix3x3 rotation;
00335   rotation[0] = x;  // x
00336   rotation[1] = y;  // y
00337   rotation[2] = z;  // z
00338   rotation = rotation.transpose();
00339   btQuaternion orientation;
00340   rotation.getRotation(orientation);
00341   return tf::Transform(orientation, position);
00342 }
00343 
00344 bool TableDetector::getPlanePoints (const pcl::PointCloud<Point> &table,
00345                                      const tf::Transform& table_plane_trans,
00346                                      sensor_msgs::PointCloud &table_points)
00347 {
00348   // Prepare the output
00349   table_points.header.frame_id = table.header.frame_id;
00350   table_points.header.stamp = table.header.stamp;
00351   table_points.points.resize (table.points.size ());
00352   for (size_t i = 0; i < table.points.size (); ++i)
00353   {
00354     table_points.points[i].x = table.points[i].x;
00355     table_points.points[i].y = table.points[i].y;
00356     table_points.points[i].z = table.points[i].z;
00357   }
00358 
00359   // Transform the data
00360   tf::TransformListener listener;
00361   tf::StampedTransform table_pose_frame(table_plane_trans, table.header.stamp,
00362                                         table.header.frame_id, "table_frame");
00363   listener.setTransform(table_pose_frame);
00364   std::string error_msg;
00365   if (!listener.canTransform("table_frame", table_points.header.frame_id, table_points.header.stamp, &error_msg))
00366   {
00367     ROS_ERROR("Can not transform point cloud from frame %s to table frame; error %s",
00368         table_points.header.frame_id.c_str(), error_msg.c_str());
00369     return false;
00370   }
00371   try
00372   {
00373     listener.transformPointCloud("table_frame", table_points, table_points);
00374   }
00375   catch (tf::TransformException ex)
00376   {
00377     ROS_ERROR("Failed to transform point cloud from frame %s into table_frame; error %s",
00378         table_points.header.frame_id.c_str(), ex.what());
00379     return false;
00380   }
00381   table_points.header.stamp = ros::Time::now();
00382   table_points.header.frame_id = "table_frame";
00383   return true;
00384 }
00385 
00386 
00387 } // object_segmentation_gui
00388 
00389
table_detector.cpp
