object_support_segmentation.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2015, Fetch Robotics Inc.
3  * Copyright 2014, Unbounded Robotics Inc.
4  * Copyright 2013, Michael E. Ferguson
5  * All Rights Reserved
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions are met:
9  *
10  * * Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * * Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in the
14  * documentation and/or other materials provided with the distribution.
15  * * The names of the authors may not be used to endorse or promote products
16  * derived from this software without specific prior written permission.
17  *
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
19  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
20  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21  * DISCLAIMED. IN NO EVENT SHALL AUTHORS BE LIABLE FOR ANY DIRECT, INDIRECT,
22  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
24  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
25  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
26  * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
27  * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 // Author: Michael Ferguson
31 
32 #include <Eigen/Eigen>
33 #include <boost/lexical_cast.hpp>
34 
35 #include <ros/ros.h>
39 
40 namespace simple_grasping
41 {
42 
44  ros::NodeHandle& nh)
45 {
46 
47  // cluster_tolerance: minimum separation distance of two objects
48  double cluster_tolerance;
49  nh.param<double>("cluster_tolerance", cluster_tolerance, 0.01);
50  extract_clusters_.setClusterTolerance(cluster_tolerance);
51 
52  // cluster_min_size: minimum size of an object
53  int cluster_min_size;
54  nh.param<int>("cluster_min_size", cluster_min_size, 50);
55  extract_clusters_.setMinClusterSize(cluster_min_size);
56 
57  // voxel grid the data before segmenting
58  double leaf_size, llimit, ulimit;
59  std::string field;
60  nh.param<double>("voxel_leaf_size", leaf_size, 0.005);
61  nh.param<double>("voxel_limit_min", llimit, 0.0);
62  nh.param<double>("voxel_limit_max", ulimit, 1.8);
63  nh.param<std::string>("voxel_field_name", field, "z");
64  voxel_grid_.setLeafSize(leaf_size, leaf_size, leaf_size);
65  voxel_grid_.setFilterFieldName(field);
66  voxel_grid_.setFilterLimits(llimit, ulimit);
67 
68  // segment objects
69  segment_.setOptimizeCoefficients(true);
70  segment_.setModelType(pcl::SACMODEL_PLANE);
71  segment_.setMaxIterations(100);
72  segment_.setDistanceThreshold(cluster_tolerance);
73 }
74 
76  const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud,
77  std::vector<grasping_msgs::Object>& objects,
78  std::vector<grasping_msgs::Object>& supports,
79  pcl::PointCloud<pcl::PointXYZRGB>& object_cloud,
80  pcl::PointCloud<pcl::PointXYZRGB>& support_cloud,
81  bool output_clouds)
82 {
83  ROS_INFO("object support segmentation starting...");
84 
85  // process the cloud with a voxel grid
86  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
87  voxel_grid_.setInputCloud(cloud);
88  voxel_grid_.filter(*cloud_filtered);
89  ROS_DEBUG("object_support_segmentation",
90  "Filtered for transformed Z, now %d points.", static_cast<int>(cloud_filtered->points.size()));
91 
92  // remove support planes
93  pcl::PointCloud<pcl::PointXYZRGB>::Ptr non_horizontal_planes(new pcl::PointCloud<pcl::PointXYZRGB>);
94  std::vector<pcl::ModelCoefficients::Ptr> plane_coefficients; // coefs of all planes found
95  int thresh = cloud_filtered->points.size()/8;
96  while (cloud_filtered->points.size() > 500)
97  {
98  pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
99  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
100 
101  // Segment the largest planar component from the remaining cloud
102  segment_.setInputCloud(cloud_filtered);
103  segment_.segment(*inliers, *coefficients);
104  if (inliers->indices.size() < (size_t) thresh) // TODO: make configurable? TODO make this based on "can we grasp object"
105  {
106  ROS_DEBUG("object_support_segmentation", "No more planes to remove.");
107  break;
108  }
109 
110  // Extract planar part for message
111  pcl::PointCloud<pcl::PointXYZRGB> plane;
112  pcl::ExtractIndices<pcl::PointXYZRGB> extract;
113  extract.setInputCloud(cloud_filtered);
114  extract.setIndices(inliers);
115  extract.setNegative(false);
116  extract.filter(plane);
117 
118  // Check plane is mostly horizontal
119  Eigen::Vector3f normal(coefficients->values[0], coefficients->values[1], coefficients->values[2]);
120  float angle = acos(Eigen::Vector3f::UnitZ().dot(normal));
121  if (angle < 0.15)
122  {
123  ROS_DEBUG("object_support_segmentation",
124  "Removing a plane with %d points.", static_cast<int>(inliers->indices.size()));
125 
126  // new support object, with cluster, bounding box, and plane
127  grasping_msgs::Object object;
128  pcl::toROSMsg(plane, object.point_cluster);
129  // give the object a temporary name
130  object.name = std::string("surface") + boost::lexical_cast<std::string>(supports.size());
131  // add shape and pose
132  shape_msgs::SolidPrimitive box;
133  geometry_msgs::Pose pose;
134  extractUnorientedBoundingBox(plane, box, pose);
135  object.primitives.push_back(box);
136  object.primitive_poses.push_back(pose);
137  // add plane
138  for (int i = 0; i < 4; ++i)
139  object.surface.coef[i] = coefficients->values[i];
140  // stamp and frame
141  object.header.stamp = ros::Time::now();
142  object.header.frame_id = cloud->header.frame_id;
143  // add support surface to object list
144  supports.push_back(object);
145 
146  if (output_clouds)
147  {
148  ROS_DEBUG("object_support_segmentation",
149  "Adding support cluster of size %d.", static_cast<int>(plane.points.size()));
150  float hue = (360.0 / 8) * supports.size();
151  colorizeCloud(plane, hue);
152  support_cloud += plane;
153  }
154 
155  // track plane for later use when determining objects
156  plane_coefficients.push_back(coefficients);
157  }
158  else
159  {
160  // Add plane to temporary point cloud so we can recover points for object extraction below
161  ROS_DEBUG("object_support_segmentation",
162  "Plane is not horizontal");
163  *non_horizontal_planes += plane;
164  }
165 
166  // Extract the non-planar parts and proceed
167  extract.setNegative(true);
168  extract.filter(*cloud_filtered);
169  }
170  ROS_DEBUG("object_support_segmentation",
171  "Cloud now %d points.", static_cast<int>(cloud_filtered->points.size()));
172 
173  // Add the non-horizontal planes back in
174  *cloud_filtered += *non_horizontal_planes;
175 
176  // Cluster
177  std::vector<pcl::PointIndices> clusters;
178  extract_clusters_.setInputCloud(cloud_filtered);
179  extract_clusters_.extract(clusters);
180  ROS_DEBUG("object_support_segmentation",
181  "Extracted %d clusters.", static_cast<int>(clusters.size()));
182 
183  extract_indices_.setInputCloud(cloud_filtered);
184  for (size_t i= 0; i < clusters.size(); i++)
185  {
186  // Extract object
187  pcl::PointCloud<pcl::PointXYZRGB> new_cloud;
188  extract_indices_.setIndices(pcl::PointIndicesPtr(new pcl::PointIndices(clusters[i])));
189  extract_indices_.filter(new_cloud);
190 
191  // Find centroid
192  Eigen::Vector4f centroid;
193  pcl::compute3DCentroid(new_cloud, centroid);
194 
195  // Compare centroid to planes to find which plane we are supported by
196  int support_plane_index = -1;
197  double support_plane_distance = 1000.0;
198  for (int p = 0; p < plane_coefficients.size(); ++p)
199  {
200  double distance = distancePointToPlane(centroid, plane_coefficients[p]);
201  if (distance > 0.0 && distance < support_plane_distance)
202  {
203  support_plane_distance = distance;
204  support_plane_index = p;
205  }
206  }
207 
208  if (support_plane_index == -1)
209  {
210  ROS_DEBUG("object_support_segmentation",
211  "No support plane found for object");
212  continue;
213  }
214 
215  // new object, with cluster and bounding box
216  grasping_msgs::Object object;
217  // set name of supporting surface
218  object.support_surface = std::string("surface") + boost::lexical_cast<std::string>(support_plane_index);
219  // add shape, pose and transformed cluster
220  shape_msgs::SolidPrimitive box;
221  geometry_msgs::Pose pose;
222  pcl::PointCloud<pcl::PointXYZRGB> projected_cloud;
223  extractShape(new_cloud, plane_coefficients[support_plane_index], projected_cloud, box, pose);
224  pcl::toROSMsg(projected_cloud, object.point_cluster);
225  object.primitives.push_back(box);
226  object.primitive_poses.push_back(pose);
227  // add stamp and frame
228  object.header.stamp = ros::Time::now();
229  object.header.frame_id = cloud->header.frame_id;
230  // add object to object list
231  objects.push_back(object);
232 
233  if (output_clouds)
234  {
235  ROS_DEBUG("object_support_segmentation",
236  "Adding an object cluster of size %d.", static_cast<int>(new_cloud.points.size()));
237  float hue = (360.0 / clusters.size()) * i;
238  colorizeCloud(new_cloud, hue);
239  object_cloud += new_cloud;
240  }
241  }
242 
243  ROS_INFO("object support segmentation done processing.");
244  return true;
245 }
246 
247 } // namespace simple_grasping
ObjectSupportSegmentation(ros::NodeHandle &nh)
Constructor, loads pipeline using ROS parameters.
pcl::VoxelGrid< pcl::PointXYZRGB > voxel_grid_
double distancePointToPlane(const PointT &point, const pcl::ModelCoefficients::Ptr plane)
Get distance from point to plane.
Definition: cloud_tools.h:71
void colorizeCloud(pcl::PointCloud< pcl::PointXYZRGB > &cloud, float hue)
Update rgb component of an XYZRGB cloud to a new HSV color.
Definition: cloud_tools.cpp:67
TFSIMD_FORCE_INLINE tfScalar distance(const Vector3 &v) const
pcl::SACSegmentation< pcl::PointXYZRGB > segment_
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
bool extractShape(const pcl::PointCloud< pcl::PointXYZRGB > &input, pcl::PointCloud< pcl::PointXYZRGB > &output, shape_msgs::SolidPrimitive &shape, geometry_msgs::Pose &pose)
Find the smallest shape primitive we can fit around this object.
bool extractUnorientedBoundingBox(const pcl::PointCloud< pcl::PointXYZRGB > &input, shape_msgs::SolidPrimitive &shape, geometry_msgs::Pose &pose)
Find a bounding box around a cloud. This method does not attempt to find best orientation and so the ...
pcl::EuclideanClusterExtraction< pcl::PointXYZRGB > extract_clusters_
TFSIMD_FORCE_INLINE tfScalar dot(const Quaternion &q1, const Quaternion &q2)
void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
static Time now()
bool segment(const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &cloud, std::vector< grasping_msgs::Object > &objects, std::vector< grasping_msgs::Object > &supports, pcl::PointCloud< pcl::PointXYZRGB > &object_cloud, pcl::PointCloud< pcl::PointXYZRGB > &support_cloud, bool output_clouds)
Split a cloud into objects and supporting surfaces.
pcl::ExtractIndices< pcl::PointXYZRGB > extract_indices_
#define ROS_DEBUG(...)


simple_grasping
Author(s): Michael Ferguson
autogenerated on Wed Jun 5 2019 20:04:47