shape_extraction.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2013-2014, Unbounded Robotics Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Unbounded Robotics, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 // Author: Michael Ferguson
31 
32 #include <math.h>
33 #include <Eigen/Eigen>
34 #include <pcl/filters/project_inliers.h>
35 #include <pcl/surface/convex_hull.h>
37 
38 namespace simple_grasping
39 {
40 
41 bool extractShape(const pcl::PointCloud<pcl::PointXYZRGB>& input,
42  const pcl::ModelCoefficients::Ptr model,
43  pcl::PointCloud<pcl::PointXYZRGB>& output,
44  shape_msgs::SolidPrimitive& shape,
45  geometry_msgs::Pose& pose)
46 {
47  // Used to decide between various shapes
48  double min_volume = 1000.0; // the minimum volume shape found thus far.
49  Eigen::Matrix3f transformation; // the transformation for the best-fit shape
50 
51  // Compute z height as maximum distance from planes
52  double height = 0.0;
53  for (size_t i = 0; i < input.size(); ++i)
54  {
55  Eigen::Vector4f pp(input[i].x, input[i].y, input[i].z, 1);
56  Eigen::Vector4f m(model->values[0], model->values[1], model->values[2], model->values[3]);
57  double distance_to_plane = fabs(pp.dot(m));
58  if (distance_to_plane > height)
59  height = distance_to_plane;
60  }
61 
62  // Project object into 2d, using plane model coefficients
63  pcl::PointCloud<pcl::PointXYZRGB>::Ptr flat(new pcl::PointCloud<pcl::PointXYZRGB>);
64  pcl::ProjectInliers<pcl::PointXYZRGB> projection;
65  projection.setModelType(pcl::SACMODEL_PLANE);
66  projection.setInputCloud(input.makeShared()); // stupid API
67  projection.setModelCoefficients(model);
68  projection.filter(*flat);
69 
70  // Rotate plane so that Z=0
71  pcl::PointCloud<pcl::PointXYZRGB>::Ptr flat_projected(new pcl::PointCloud<pcl::PointXYZRGB>);
72  Eigen::Vector3f normal(model->values[0], model->values[1], model->values[2]);
73  Eigen::Quaternionf qz; qz.setFromTwoVectors(normal, Eigen::Vector3f::UnitZ());
74  Eigen::Matrix3f plane_rotation = qz.toRotationMatrix();
75  Eigen::Matrix3f inv_plane_rotation = plane_rotation.inverse();
76 
77  for (size_t i = 0; i < flat->size(); ++i)
78  {
79  pcl::PointXYZRGB p;
80  p.getVector3fMap() = plane_rotation * (*flat)[i].getVector3fMap();
81  flat_projected->push_back(p);
82  }
83 
84  // Find the convex hull
85  pcl::PointCloud<pcl::PointXYZRGB> hull;
86  pcl::ConvexHull<pcl::PointXYZRGB> convex_hull;
87  convex_hull.setInputCloud(flat_projected);
88  convex_hull.setDimension(2);
89  convex_hull.reconstruct(hull);
90 
91  // Try fitting a rectangle
92  shape_msgs::SolidPrimitive rect; // the best-fit rectangle
93  rect.type = rect.BOX;
94  rect.dimensions.resize(3);
95  for (size_t i = 0; i < hull.size() - 1; ++i)
96  {
97  // For each pair of hull points, determine the angle
98  double rise = hull[i+1].y - hull[i].y;
99  double run = hull[i+1].x - hull[i].x;
100  // and normalize..
101  {
102  double l = sqrt((rise * rise) + (run * run));
103  rise = rise/l;
104  run = run/l;
105  }
106 
107  // Build rotation matrix from change of basis
108  Eigen::Matrix3f rotation;
109  rotation(0, 0) = run;
110  rotation(0, 1) = rise;
111  rotation(0, 2) = 0.0;
112  rotation(1, 0) = -rise;
113  rotation(1, 1) = run;
114  rotation(1, 2) = 0.0;
115  rotation(2, 0) = 0.0;
116  rotation(2, 1) = 0.0;
117  rotation(2, 2) = 1.0;
118  Eigen::Matrix3f inv_rotation = rotation.inverse();
119 
120  // Project hull to new coordinate system
121  pcl::PointCloud<pcl::PointXYZRGB> projected_cloud;
122  for (size_t j = 0; j < hull.size(); ++j)
123  {
124  pcl::PointXYZRGB p;
125  p.getVector3fMap() = rotation * hull[j].getVector3fMap();
126  projected_cloud.push_back(p);
127  }
128 
129  // Compute min/max
130  double x_min = 1000.0;
131  double x_max = -1000.0;
132  double y_min = 1000.0;
133  double y_max = -1000.0;
134  for (size_t j = 0; j < projected_cloud.size(); ++j)
135  {
136  if (projected_cloud[j].x < x_min)
137  x_min = projected_cloud[j].x;
138  if (projected_cloud[j].x > x_max)
139  x_max = projected_cloud[j].x;
140 
141  if (projected_cloud[j].y < y_min)
142  y_min = projected_cloud[j].y;
143  if (projected_cloud[j].y > y_max)
144  y_max = projected_cloud[j].y;
145  }
146 
147  // Is this the best estimate?
148  double area = (x_max - x_min) * (y_max - y_min);
149  if (area*height < min_volume)
150  {
151  transformation = inv_plane_rotation * inv_rotation;
152 
153  rect.dimensions[0] = (x_max - x_min);
154  rect.dimensions[1] = (y_max - y_min);
155  rect.dimensions[2] = height;
156 
157  Eigen::Vector3f pose3f((x_max + x_min)/2.0, (y_max + y_min)/2.0,
158  projected_cloud[0].z + height/2.0);
159  pose3f = transformation * pose3f;
160  pose.position.x = pose3f(0);
161  pose.position.y = pose3f(1);
162  pose.position.z = pose3f(2);
163 
164  Eigen::Quaternionf q(transformation);
165  pose.orientation.x = q.x();
166  pose.orientation.y = q.y();
167  pose.orientation.z = q.z();
168  pose.orientation.w = q.w();
169 
170  min_volume = area * height;
171  shape = rect;
172  }
173  }
174 
175  // Try fitting a cylinder
176  shape_msgs::SolidPrimitive cylinder; // the best-fit cylinder
177  cylinder.type = cylinder.CYLINDER;
178  cylinder.dimensions.resize(2);
179  for (size_t i = 0; i < hull.size(); ++i)
180  {
181  for (size_t j = i + 1; j < hull.size(); ++j)
182  {
183  // For each pair of hull points determine the center point
184  // between them as a possible cylinder
185  pcl::PointXYZRGB p;
186  p.x = (hull[i].x + hull[j].x) / 2.0;
187  p.y = (hull[i].y + hull[j].y) / 2.0;
188  double radius = 0.0;
189  // Find radius from this point
190  for (size_t k = 0; k < hull.size(); ++k)
191  {
192  double dx = hull[k].x - p.x;
193  double dy = hull[k].y - p.y;
194  double r = sqrt((dx * dx) + (dy * dy));
195  if (r > radius)
196  radius = r;
197  }
198  // Is this cylinder the best match?
199  double volume = M_PI * radius * radius * height;
200  if (volume < min_volume)
201  {
202  transformation = inv_plane_rotation;
203 
204  cylinder.dimensions[0] = height;
205  cylinder.dimensions[1] = radius;
206 
207  Eigen::Vector3f pose3f(p.x, p.y, hull[0].z + height/2.0);
208  pose3f = transformation * pose3f;
209  pose.position.x = pose3f(0);
210  pose.position.y = pose3f(1);
211  pose.position.z = pose3f(2);
212 
213  min_volume = volume;
214  shape = cylinder;
215  }
216  }
217  }
218 
219  // TODO: Try fitting a sphere?
220 
221  // Project input to new frame
222  Eigen::Vector3f origin(pose.position.x, pose.position.y, pose.position.z);
223  for (size_t j = 0; j < input.size(); ++j)
224  {
225  pcl::PointXYZRGB p;
226  p.getVector3fMap() = transformation * (input[j].getVector3fMap() - origin);
227  output.push_back(p);
228  }
229  return true;
230 }
231 
232 bool extractShape(const pcl::PointCloud<pcl::PointXYZRGB>& input,
233  pcl::PointCloud<pcl::PointXYZRGB>& output,
234  shape_msgs::SolidPrimitive& shape,
235  geometry_msgs::Pose& pose)
236 {
237  // Find lowest point, use as z height
238  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
239  coefficients->values.resize(4);
240  coefficients->values[0] = 0.0;
241  coefficients->values[1] = 0.0;
242  coefficients->values[2] = 1.0;
243  coefficients->values[3] = 1000.0; // z-height
244  for (size_t i = 0; i < input.size(); ++i)
245  {
246  if (input[i].z < coefficients->values[3])
247  coefficients->values[3] = input[i].z;
248  }
249  coefficients->values[3] = -coefficients->values[3];
250  return extractShape(input, coefficients, output, shape, pose);
251 }
252 
253 bool extractUnorientedBoundingBox(const pcl::PointCloud<pcl::PointXYZRGB>& input,
254  shape_msgs::SolidPrimitive& shape,
255  geometry_msgs::Pose& pose)
256 {
257  double x_min = 1000.0;
258  double x_max = -1000.0;
259  double y_min = 1000.0;
260  double y_max = -1000.0;
261  double z_min = 1000.0;
262  double z_max = -1000.0;
263 
264  for (size_t i = 0; i < input.size(); ++i)
265  {
266  if (input[i].x < x_min)
267  x_min = input[i].x;
268  if (input[i].x > x_max)
269  x_max = input[i].x;
270 
271  if (input[i].y < y_min)
272  y_min = input[i].y;
273  if (input[i].y > y_max)
274  y_max = input[i].y;
275 
276  if (input[i].z < z_min)
277  z_min = input[i].z;
278  if (input[i].z > z_max)
279  z_max = input[i].z;
280  }
281 
282  pose.position.x = (x_min + x_max)/2.0;
283  pose.position.y = (y_min + y_max)/2.0;
284  pose.position.z = (z_min + z_max)/2.0;
285 
286  shape.type = shape.BOX;
287  shape.dimensions.push_back(x_max-x_min);
288  shape.dimensions.push_back(y_max-y_min);
289  shape.dimensions.push_back(z_max-z_min);
290 
291  return true;
292 }
293 
294 } // namespace simple_grasping
std::vector< double > values
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 ...
TFSIMD_FORCE_INLINE const tfScalar & x() const
TFSIMD_FORCE_INLINE const tfScalar & z() const
void run(ClassLoader *loader)


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