shape_extraction.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2013-2014, Unbounded Robotics Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Unbounded Robotics, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 // Author: Michael Ferguson
00031 
00032 #include <math.h>
00033 #include <Eigen/Eigen>
00034 #include <pcl/filters/project_inliers.h>
00035 #include <pcl/surface/convex_hull.h>
00036 #include <simple_grasping/shape_extraction.h>
00037 
00038 namespace simple_grasping
00039 {
00040 
00041 bool extractShape(const pcl::PointCloud<pcl::PointXYZRGB>& input,
00042                   const pcl::ModelCoefficients::Ptr model,
00043                   pcl::PointCloud<pcl::PointXYZRGB>& output,
00044                   shape_msgs::SolidPrimitive& shape,
00045                   geometry_msgs::Pose& pose)
00046 {
00047   // Used to decide between various shapes
00048   double min_volume = 1000.0;  // the minimum volume shape found thus far.
00049   Eigen::Matrix3f transformation;  // the transformation for the best-fit shape
00050 
00051   // Compute z height as maximum distance from planes
00052   double height = 0.0;
00053   for (size_t i = 0; i < input.size(); ++i)
00054   {
00055     Eigen::Vector4f pp(input[i].x, input[i].y, input[i].z, 1);
00056     Eigen::Vector4f m(model->values[0], model->values[1], model->values[2], model->values[3]);
00057     double distance_to_plane = fabs(pp.dot(m));
00058     if (distance_to_plane > height)
00059       height = distance_to_plane;
00060   }
00061 
00062   // Project object into 2d, using plane model coefficients
00063   pcl::PointCloud<pcl::PointXYZRGB>::Ptr flat(new pcl::PointCloud<pcl::PointXYZRGB>);
00064   pcl::ProjectInliers<pcl::PointXYZRGB> projection;
00065   projection.setModelType(pcl::SACMODEL_PLANE);
00066   projection.setInputCloud(input.makeShared());  // stupid API
00067   projection.setModelCoefficients(model);
00068   projection.filter(*flat);
00069 
00070   // Rotate plane so that Z=0
00071   pcl::PointCloud<pcl::PointXYZRGB>::Ptr flat_projected(new pcl::PointCloud<pcl::PointXYZRGB>);
00072   Eigen::Vector3f normal(model->values[0], model->values[1], model->values[2]);
00073   Eigen::Quaternionf qz; qz.setFromTwoVectors(normal, Eigen::Vector3f::UnitZ());
00074   Eigen::Matrix3f plane_rotation = qz.toRotationMatrix();
00075   Eigen::Matrix3f inv_plane_rotation = plane_rotation.inverse();
00076 
00077   for (size_t i = 0; i < flat->size(); ++i)
00078   {
00079     pcl::PointXYZRGB p;
00080     p.getVector3fMap() = plane_rotation * (*flat)[i].getVector3fMap();
00081     flat_projected->push_back(p);
00082   }
00083 
00084   // Find the convex hull
00085   pcl::PointCloud<pcl::PointXYZRGB> hull;
00086   pcl::ConvexHull<pcl::PointXYZRGB> convex_hull;
00087   convex_hull.setInputCloud(flat_projected);
00088   convex_hull.setDimension(2);
00089   convex_hull.reconstruct(hull);
00090 
00091   // Try fitting a rectangle
00092   shape_msgs::SolidPrimitive rect;  // the best-fit rectangle
00093   rect.type = rect.BOX;
00094   rect.dimensions.resize(3);
00095   for (size_t i = 0; i < hull.size() - 1; ++i)
00096   {
00097     // For each pair of hull points, determine the angle
00098     double rise = hull[i+1].y - hull[i].y;
00099     double run = hull[i+1].x - hull[i].x;
00100     // and normalize..
00101     {
00102       double l = sqrt((rise * rise) + (run * run));
00103       rise = rise/l;
00104       run = run/l;
00105     }
00106 
00107     // Build rotation matrix from change of basis
00108     Eigen::Matrix3f rotation;
00109     rotation(0, 0) = run;
00110     rotation(0, 1) = rise;
00111     rotation(0, 2) = 0.0;
00112     rotation(1, 0) = -rise;
00113     rotation(1, 1) = run;
00114     rotation(1, 2) = 0.0;
00115     rotation(2, 0) = 0.0;
00116     rotation(2, 1) = 0.0;
00117     rotation(2, 2) = 1.0;
00118     Eigen::Matrix3f inv_rotation = rotation.inverse();
00119 
00120     // Project hull to new coordinate system
00121     pcl::PointCloud<pcl::PointXYZRGB> projected_cloud;
00122     for (size_t j = 0; j < hull.size(); ++j)
00123     {
00124       pcl::PointXYZRGB p;
00125       p.getVector3fMap() = rotation * hull[j].getVector3fMap();
00126       projected_cloud.push_back(p);
00127     }
00128 
00129     // Compute min/max
00130     double x_min = 1000.0;
00131     double x_max = -1000.0;
00132     double y_min = 1000.0;
00133     double y_max = -1000.0;
00134     for (size_t j = 0; j < projected_cloud.size(); ++j)
00135     {
00136       if (projected_cloud[j].x < x_min)
00137         x_min = projected_cloud[j].x;
00138       if (projected_cloud[j].x > x_max)
00139         x_max = projected_cloud[j].x;
00140 
00141       if (projected_cloud[j].y < y_min)
00142         y_min = projected_cloud[j].y;
00143       if (projected_cloud[j].y > y_max)
00144         y_max = projected_cloud[j].y;
00145     }
00146 
00147     // Is this the best estimate?
00148     double area = (x_max - x_min) * (y_max - y_min);
00149     if (area*height < min_volume)
00150     {
00151       transformation = inv_plane_rotation * inv_rotation;
00152 
00153       rect.dimensions[0] = (x_max - x_min);
00154       rect.dimensions[1] = (y_max - y_min);
00155       rect.dimensions[2] = height;
00156 
00157       Eigen::Vector3f pose3f((x_max + x_min)/2.0, (y_max + y_min)/2.0,
00158                              projected_cloud[0].z + height/2.0);
00159       pose3f = transformation * pose3f;
00160       pose.position.x = pose3f(0);
00161       pose.position.y = pose3f(1);
00162       pose.position.z = pose3f(2);
00163 
00164       Eigen::Quaternionf q(transformation);
00165       pose.orientation.x = q.x();
00166       pose.orientation.y = q.y();
00167       pose.orientation.z = q.z();
00168       pose.orientation.w = q.w();
00169 
00170       min_volume = area * height;
00171       shape = rect;
00172     }
00173   }
00174 
00175   // Try fitting a cylinder
00176   shape_msgs::SolidPrimitive cylinder;  // the best-fit cylinder
00177   cylinder.type = cylinder.CYLINDER;
00178   cylinder.dimensions.resize(2);
00179   for (size_t i = 0; i < hull.size(); ++i)
00180   {
00181     for (size_t j = i + 1; j < hull.size(); ++j)
00182     {
00183       // For each pair of hull points determine the center point
00184       //  between them as a possible cylinder
00185       pcl::PointXYZRGB p;
00186       p.x = (hull[i].x + hull[j].x) / 2.0;
00187       p.y = (hull[i].y + hull[j].y) / 2.0;
00188       double radius = 0.0;
00189       // Find radius from this point
00190       for (size_t k = 0; k < hull.size(); ++k)
00191       {
00192         double dx = hull[k].x - p.x;
00193         double dy = hull[k].y - p.y;
00194         double r = sqrt((dx * dx) + (dy * dy));
00195         if (r > radius)
00196           radius = r;
00197       }
00198       // Is this cylinder the best match?
00199       double volume = M_PI * radius * radius * height;
00200       if (volume < min_volume)
00201       {
00202         transformation = inv_plane_rotation;
00203 
00204         cylinder.dimensions[0] = height;
00205         cylinder.dimensions[1] = radius;
00206 
00207         Eigen::Vector3f pose3f(p.x, p.y, hull[0].z + height/2.0);
00208         pose3f = transformation * pose3f;
00209         pose.position.x = pose3f(0);
00210         pose.position.y = pose3f(1);
00211         pose.position.z = pose3f(2);
00212 
00213         min_volume = volume;
00214         shape = cylinder;
00215       }
00216     }
00217   }
00218 
00219   // TODO: Try fitting a sphere?
00220 
00221   // Project input to new frame
00222   Eigen::Vector3f origin(pose.position.x, pose.position.y, pose.position.z);
00223   for (size_t j = 0; j < input.size(); ++j)
00224   {
00225     pcl::PointXYZRGB p;
00226     p.getVector3fMap() = transformation * (input[j].getVector3fMap() - origin);
00227     output.push_back(p);
00228   }
00229   return true;
00230 }
00231 
00232 bool extractShape(const pcl::PointCloud<pcl::PointXYZRGB>& input,
00233                   pcl::PointCloud<pcl::PointXYZRGB>& output,
00234                   shape_msgs::SolidPrimitive& shape,
00235                   geometry_msgs::Pose& pose)
00236 {
00237   // Find lowest point, use as z height
00238   pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
00239   coefficients->values.resize(4);
00240   coefficients->values[0] = 0.0;
00241   coefficients->values[1] = 0.0;
00242   coefficients->values[2] = 1.0;
00243   coefficients->values[3] = 1000.0;  // z-height
00244   for (size_t i = 0; i < input.size(); ++i)
00245   {
00246     if (input[i].z < coefficients->values[3])
00247       coefficients->values[3] = input[i].z;
00248   }
00249   coefficients->values[3] = -coefficients->values[3];
00250   return extractShape(input, coefficients, output, shape, pose);
00251 }
00252 
00253 bool extractUnorientedBoundingBox(const pcl::PointCloud<pcl::PointXYZRGB>& input,
00254                                   shape_msgs::SolidPrimitive& shape,
00255                                   geometry_msgs::Pose& pose)
00256 {
00257   double x_min = 1000.0;
00258   double x_max = -1000.0;
00259   double y_min = 1000.0;
00260   double y_max = -1000.0;
00261   double z_min = 1000.0;
00262   double z_max = -1000.0;
00263 
00264   for (size_t i = 0; i < input.size(); ++i)
00265   {
00266     if (input[i].x < x_min)
00267       x_min = input[i].x;
00268     if (input[i].x > x_max)
00269       x_max = input[i].x;
00270 
00271     if (input[i].y < y_min)
00272       y_min = input[i].y;
00273     if (input[i].y > y_max)
00274       y_max = input[i].y;
00275 
00276     if (input[i].z < z_min)
00277       z_min = input[i].z;
00278     if (input[i].z > z_max)
00279       z_max = input[i].z;
00280   }
00281 
00282   pose.position.x = (x_min + x_max)/2.0;
00283   pose.position.y = (y_min + y_max)/2.0;
00284   pose.position.z = (z_min + z_max)/2.0;
00285 
00286   shape.type = shape.BOX;
00287   shape.dimensions.push_back(x_max-x_min);
00288   shape.dimensions.push_back(y_max-y_min);
00289   shape.dimensions.push_back(z_max-z_min);
00290 
00291   return true;
00292 }
00293 
00294 }  // namespace simple_grasping


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