object_support_segmentation.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2015, Fetch Robotics Inc.
00003  * Copyright 2014, Unbounded Robotics Inc.
00004  * Copyright 2013, Michael E. Ferguson
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 are met:
00009  *
00010  *     * Redistributions of source code must retain the above copyright
00011  *       notice, this list of conditions and the following disclaimer.
00012  *     * Redistributions in binary form must reproduce the above copyright
00013  *       notice, this list of conditions and the following disclaimer in the
00014  *       documentation and/or other materials provided with the distribution.
00015  *     * The names of the authors may not be used to endorse or promote products
00016  *       derived from this software without specific prior written permission.
00017  *
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00019  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00020  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00021  * DISCLAIMED. IN NO EVENT SHALL AUTHORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00022  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00023  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00024  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00025  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00026  * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00027  * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 // Author: Michael Ferguson
00031 
00032 #include <Eigen/Eigen>
00033 #include <boost/lexical_cast.hpp>
00034 
00035 #include <ros/ros.h>
00036 #include <simple_grasping/cloud_tools.h>
00037 #include <simple_grasping/shape_extraction.h>
00038 #include <simple_grasping/object_support_segmentation.h>
00039 
00040 namespace simple_grasping
00041 {
00042 
00043 ObjectSupportSegmentation::ObjectSupportSegmentation(
00044   ros::NodeHandle& nh)
00045 {
00046 
00047   // cluster_tolerance: minimum separation distance of two objects
00048   double cluster_tolerance;
00049   nh.param<double>("cluster_tolerance", cluster_tolerance, 0.01);
00050   extract_clusters_.setClusterTolerance(cluster_tolerance);
00051 
00052   // cluster_min_size: minimum size of an object
00053   int cluster_min_size;
00054   nh.param<int>("cluster_min_size", cluster_min_size, 50);
00055   extract_clusters_.setMinClusterSize(cluster_min_size);
00056 
00057   // voxel grid the data before segmenting
00058   double leaf_size, llimit, ulimit;
00059   std::string field;
00060   nh.param<double>("voxel_leaf_size", leaf_size, 0.005);
00061   nh.param<double>("voxel_limit_min", llimit, 0.0);
00062   nh.param<double>("voxel_limit_max", ulimit, 1.8);
00063   nh.param<std::string>("voxel_field_name", field, "z");
00064   voxel_grid_.setLeafSize(leaf_size, leaf_size, leaf_size);
00065   voxel_grid_.setFilterFieldName(field);
00066   voxel_grid_.setFilterLimits(llimit, ulimit);
00067 
00068   // segment objects
00069   segment_.setOptimizeCoefficients(true);
00070   segment_.setModelType(pcl::SACMODEL_PLANE);
00071   segment_.setMaxIterations(100);
00072   segment_.setDistanceThreshold(cluster_tolerance);
00073 }
00074 
00075 bool ObjectSupportSegmentation::segment(
00076   const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud,
00077   std::vector<grasping_msgs::Object>& objects,
00078   std::vector<grasping_msgs::Object>& supports,
00079   pcl::PointCloud<pcl::PointXYZRGB>& object_cloud,
00080   pcl::PointCloud<pcl::PointXYZRGB>& support_cloud,
00081   bool output_clouds)
00082 {
00083   ROS_INFO("object support segmentation starting...");
00084 
00085   // process the cloud with a voxel grid
00086   pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
00087   voxel_grid_.setInputCloud(cloud);
00088   voxel_grid_.filter(*cloud_filtered);
00089   ROS_DEBUG("object_support_segmentation",
00090             "Filtered for transformed Z, now %d points.", static_cast<int>(cloud_filtered->points.size()));
00091 
00092   // remove support planes
00093   pcl::PointCloud<pcl::PointXYZRGB>::Ptr non_horizontal_planes(new pcl::PointCloud<pcl::PointXYZRGB>);
00094   std::vector<pcl::ModelCoefficients::Ptr> plane_coefficients;  // coefs of all planes found
00095   int thresh = cloud_filtered->points.size()/8;
00096   while (cloud_filtered->points.size() > 500)
00097   {
00098     pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
00099     pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
00100 
00101     // Segment the largest planar component from the remaining cloud
00102     segment_.setInputCloud(cloud_filtered);
00103     segment_.segment(*inliers, *coefficients);
00104     if (inliers->indices.size() < (size_t) thresh)  // TODO: make configurable? TODO make this based on "can we grasp object"
00105     {
00106       ROS_DEBUG("object_support_segmentation", "No more planes to remove.");
00107       break;
00108     }
00109 
00110     // Extract planar part for message
00111     pcl::PointCloud<pcl::PointXYZRGB> plane;
00112     pcl::ExtractIndices<pcl::PointXYZRGB> extract;
00113     extract.setInputCloud(cloud_filtered);
00114     extract.setIndices(inliers);
00115     extract.setNegative(false);
00116     extract.filter(plane);
00117 
00118     // Check plane is mostly horizontal
00119     Eigen::Vector3f normal(coefficients->values[0], coefficients->values[1], coefficients->values[2]);
00120     float angle = acos(Eigen::Vector3f::UnitZ().dot(normal));
00121     if (angle < 0.15)
00122     {
00123       ROS_DEBUG("object_support_segmentation",
00124                 "Removing a plane with %d points.", static_cast<int>(inliers->indices.size()));
00125 
00126       // new support object, with cluster, bounding box, and plane
00127       grasping_msgs::Object object;
00128       pcl::toROSMsg(plane, object.point_cluster);
00129       // give the object a temporary name
00130       object.name = std::string("surface") + boost::lexical_cast<std::string>(supports.size());
00131       // add shape and pose
00132       shape_msgs::SolidPrimitive box;
00133       geometry_msgs::Pose pose;
00134       extractUnorientedBoundingBox(plane, box, pose);
00135       object.primitives.push_back(box);
00136       object.primitive_poses.push_back(pose);
00137       // add plane
00138       for (int i = 0; i < 4; ++i)
00139         object.surface.coef[i] = coefficients->values[i];
00140       // stamp and frame
00141       object.header.stamp = ros::Time::now();
00142       object.header.frame_id = cloud->header.frame_id;
00143       // add support surface to object list
00144       supports.push_back(object);
00145 
00146       if (output_clouds)
00147       {
00148         ROS_DEBUG("object_support_segmentation",
00149                   "Adding support cluster of size %d.", static_cast<int>(plane.points.size()));
00150         float hue = (360.0 / 8) * supports.size();
00151         colorizeCloud(plane, hue);
00152         support_cloud += plane;
00153       }
00154 
00155       // track plane for later use when determining objects
00156       plane_coefficients.push_back(coefficients);
00157     }
00158     else
00159     {
00160       // Add plane to temporary point cloud so we can recover points for object extraction below
00161       ROS_DEBUG("object_support_segmentation",
00162                 "Plane is not horizontal");
00163       *non_horizontal_planes += plane;
00164     }
00165 
00166     // Extract the non-planar parts and proceed
00167     extract.setNegative(true);
00168     extract.filter(*cloud_filtered);
00169   }
00170   ROS_DEBUG("object_support_segmentation",
00171             "Cloud now %d points.", static_cast<int>(cloud_filtered->points.size()));
00172 
00173   // Add the non-horizontal planes back in
00174   *cloud_filtered += *non_horizontal_planes;
00175 
00176   // Cluster
00177   std::vector<pcl::PointIndices> clusters;
00178   extract_clusters_.setInputCloud(cloud_filtered);
00179   extract_clusters_.extract(clusters);
00180   ROS_DEBUG("object_support_segmentation",
00181             "Extracted %d clusters.", static_cast<int>(clusters.size()));
00182 
00183   extract_indices_.setInputCloud(cloud_filtered);
00184   for (size_t i= 0; i < clusters.size(); i++)
00185   {
00186     // Extract object
00187     pcl::PointCloud<pcl::PointXYZRGB> new_cloud;
00188     extract_indices_.setIndices(pcl::PointIndicesPtr(new pcl::PointIndices(clusters[i])));
00189     extract_indices_.filter(new_cloud);
00190 
00191     // Find centroid
00192     Eigen::Vector4f centroid;
00193     pcl::compute3DCentroid(new_cloud, centroid);
00194 
00195     // Compare centroid to planes to find which plane we are supported by
00196     int support_plane_index = -1;
00197     double support_plane_distance = 1000.0;
00198     for (int p = 0; p < plane_coefficients.size(); ++p)
00199     {
00200       double distance = distancePointToPlane(centroid, plane_coefficients[p]);
00201       if (distance > 0.0 && distance < support_plane_distance)
00202       {
00203         support_plane_distance = distance;
00204         support_plane_index = p;
00205       }
00206     }
00207 
00208     if (support_plane_index == -1)
00209     {
00210       ROS_DEBUG("object_support_segmentation",
00211                 "No support plane found for object");
00212       continue;
00213     }
00214 
00215     // new object, with cluster and bounding box
00216     grasping_msgs::Object object;
00217     // set name of supporting surface
00218     object.support_surface = std::string("surface") + boost::lexical_cast<std::string>(support_plane_index);
00219     // add shape, pose and transformed cluster
00220     shape_msgs::SolidPrimitive box;
00221     geometry_msgs::Pose pose;
00222     pcl::PointCloud<pcl::PointXYZRGB> projected_cloud;
00223     extractShape(new_cloud, plane_coefficients[support_plane_index], projected_cloud, box, pose);
00224     pcl::toROSMsg(projected_cloud, object.point_cluster);
00225     object.primitives.push_back(box);
00226     object.primitive_poses.push_back(pose);
00227     // add stamp and frame
00228     object.header.stamp = ros::Time::now();
00229     object.header.frame_id = cloud->header.frame_id;
00230     // add object to object list
00231     objects.push_back(object);
00232 
00233     if (output_clouds)
00234     {
00235       ROS_DEBUG("object_support_segmentation",
00236                 "Adding an object cluster of size %d.", static_cast<int>(new_cloud.points.size()));
00237       float hue = (360.0 / clusters.size()) * i;
00238       colorizeCloud(new_cloud, hue);
00239       object_cloud += new_cloud;
00240     }
00241   }
00242 
00243   ROS_INFO("object support segmentation done processing.");
00244   return true;
00245 }
00246 
00247 }  // namespace simple_grasping


simple_grasping
Author(s): Michael Ferguson
autogenerated on Fri Aug 26 2016 13:17:24