Point Cloud Library (PCL)  1.3.1
extract_polygonal_prism_data.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
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
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: extract_polygonal_prism_data.hpp 1370 2011-06-19 01:06:01Z jspricke $
00035  *
00036  */
00037 
00038 #ifndef PCL_SEGMENTATION_IMPL_EXTRACT_POLYGONAL_PRISM_DATA_H_
00039 #define PCL_SEGMENTATION_IMPL_EXTRACT_POLYGONAL_PRISM_DATA_H_
00040 
00041 #include "pcl/segmentation/extract_polygonal_prism_data.h"
00042 #include "pcl/common/centroid.h"
00043 #include "pcl/common/eigen.h"
00044 
00046 template <typename PointT> bool
00047 pcl::isPointIn2DPolygon (const PointT &point, const pcl::PointCloud<PointT> &polygon)
00048 {
00049   // Compute the plane coefficients
00050   Eigen::Vector4f model_coefficients;
00051   EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
00052   Eigen::Vector4f xyz_centroid;
00053 
00054   // Estimate the XYZ centroid
00055   compute3DCentroid (polygon, xyz_centroid);
00056 
00057   // Compute the 3x3 covariance matrix
00058   computeCovarianceMatrix (polygon, xyz_centroid, covariance_matrix);
00059 
00060   // Compute the model coefficients
00061   EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
00062   EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
00063   eigen33 (covariance_matrix, eigen_vectors, eigen_values);
00064 
00065   model_coefficients[0] = eigen_vectors (0, 0);
00066   model_coefficients[1] = eigen_vectors (1, 0);
00067   model_coefficients[2] = eigen_vectors (2, 0);
00068   model_coefficients[3] = 0;
00069 
00070   // Hessian form (D = nc . p_plane (centroid here) + p)
00071   model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
00072 
00073   float distance_to_plane = model_coefficients[0] * point.x + 
00074                             model_coefficients[1] * point.y + 
00075                             model_coefficients[2] * point.z + 
00076                             model_coefficients[3];
00077   PointT ppoint;
00078   // Calculate the projection of the point on the plane
00079   ppoint.x = point.x - distance_to_plane * model_coefficients[0];
00080   ppoint.y = point.y - distance_to_plane * model_coefficients[1];
00081   ppoint.z = point.z - distance_to_plane * model_coefficients[2];
00082 
00083   // Create a X-Y projected representation for within bounds polygonal checking
00084   int k0, k1, k2;
00085   // Determine the best plane to project points onto
00086   k0 = (fabs (model_coefficients[0] ) > fabs (model_coefficients[1])) ? 0  : 1;
00087   k0 = (fabs (model_coefficients[k0]) > fabs (model_coefficients[2])) ? k0 : 2;
00088   k1 = (k0 + 1) % 3;
00089   k2 = (k0 + 2) % 3;
00090   // Project the convex hull
00091   pcl::PointCloud<PointT> xy_polygon;
00092   xy_polygon.points.resize (polygon.points.size ());
00093   for (size_t i = 0; i < polygon.points.size (); ++i)
00094   {
00095     Eigen::Vector4f pt (polygon.points[i].x, polygon.points[i].y, polygon.points[i].z, 0);
00096     xy_polygon.points[i].x = pt[k1];
00097     xy_polygon.points[i].y = pt[k2];
00098     xy_polygon.points[i].z = 0;
00099   }
00100   PointT xy_point;
00101   xy_point.z = 0;
00102   Eigen::Vector4f pt (ppoint.x, ppoint.y, ppoint.z, 0);
00103   xy_point.x = pt[k1];
00104   xy_point.y = pt[k2];
00105 
00106   return (pcl::isXYPointIn2DXYPolygon (xy_point, xy_polygon));
00107 }
00108 
00110 template <typename PointT> bool
00111 pcl::isXYPointIn2DXYPolygon (const PointT &point, const pcl::PointCloud<PointT> &polygon)
00112 {
00113   bool in_poly = false;
00114   double x1, x2, y1, y2;
00115 
00116   int nr_poly_points = polygon.points.size ();
00117   double xold = polygon.points[nr_poly_points - 1].x;
00118   double yold = polygon.points[nr_poly_points - 1].y;
00119   for (int i = 0; i < nr_poly_points; i++)
00120   {
00121     double xnew = polygon.points[i].x;
00122     double ynew = polygon.points[i].y;
00123     if (xnew > xold)
00124     {
00125       x1 = xold;
00126       x2 = xnew;
00127       y1 = yold;
00128       y2 = ynew;
00129     }
00130     else
00131     {
00132       x1 = xnew;
00133       x2 = xold;
00134       y1 = ynew;
00135       y2 = yold;
00136     }
00137 
00138     if ( (xnew < point.x) == (point.x <= xold) && (point.y - y1) * (x2 - x1) < (y2 - y1) * (point.x - x1) )
00139     {
00140       in_poly = !in_poly;
00141     }
00142     xold = xnew;
00143     yold = ynew;
00144   }
00145   return (in_poly);
00146 }
00147 
00149 template <typename PointT> void
00150 pcl::ExtractPolygonalPrismData<PointT>::segment (pcl::PointIndices &output)
00151 {
00152   output.header = input_->header;
00153 
00154   if (!initCompute ()) 
00155   {
00156     output.indices.clear ();
00157     return;
00158   }
00159 
00160   if ((int)planar_hull_->points.size () < min_pts_hull_)
00161   {
00162     PCL_ERROR ("[pcl::%s::segment] Not enough points (%lu) in the hull!\n", getClassName ().c_str (), (unsigned long)planar_hull_->points.size ());
00163     output.indices.clear ();
00164     return;
00165   }
00166 
00167   // Compute the plane coefficients
00168   Eigen::Vector4f model_coefficients;
00169   EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
00170   Eigen::Vector4f xyz_centroid;
00171 
00172   // Estimate the XYZ centroid
00173   compute3DCentroid (*planar_hull_, xyz_centroid);
00174 
00175   // Compute the 3x3 covariance matrix
00176   computeCovarianceMatrix (*planar_hull_, xyz_centroid, covariance_matrix);
00177 
00178   // Compute the model coefficients
00179   EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
00180   EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
00181   eigen33 (covariance_matrix, eigen_vectors, eigen_values);
00182 
00183   model_coefficients[0] = eigen_vectors (0, 0);
00184   model_coefficients[1] = eigen_vectors (1, 0);
00185   model_coefficients[2] = eigen_vectors (2, 0);
00186   model_coefficients[3] = 0;
00187 
00188   // Hessian form (D = nc . p_plane (centroid here) + p)
00189   model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
00190 
00191   // Need to flip the plane normal towards the viewpoint
00192   Eigen::Vector4f vp (vpx_, vpy_, vpz_, 0);
00193   // See if we need to flip any plane normals
00194   vp -= planar_hull_->points[0].getVector4fMap ();
00195   vp[3] = 0;
00196   // Dot product between the (viewpoint - point) and the plane normal
00197   float cos_theta = vp.dot (model_coefficients);
00198   // Flip the plane normal
00199   if (cos_theta < 0)
00200   {                                                                
00201     model_coefficients *= -1;
00202     model_coefficients[3] = 0;
00203     // Hessian form (D = nc . p_plane (centroid here) + p)
00204     model_coefficients[3] = -1 * (model_coefficients.dot (planar_hull_->points[0].getVector4fMap ()));
00205   }
00206     
00207   // Project all points
00208   PointCloud projected_points;
00209   SampleConsensusModelPlane<PointT> sacmodel (input_);
00210   sacmodel.projectPoints (*indices_, model_coefficients, projected_points, false);
00211 
00212   // Create a X-Y projected representation for within bounds polygonal checking
00213   int k0, k1, k2;
00214   // Determine the best plane to project points onto
00215   k0 = (fabs (model_coefficients[0] ) > fabs (model_coefficients[1])) ? 0  : 1;
00216   k0 = (fabs (model_coefficients[k0]) > fabs (model_coefficients[2])) ? k0 : 2;
00217   k1 = (k0 + 1) % 3;
00218   k2 = (k0 + 2) % 3;
00219   // Project the convex hull
00220   pcl::PointCloud<PointT> polygon;
00221   polygon.points.resize (planar_hull_->points.size ());
00222   for (size_t i = 0; i < planar_hull_->points.size (); ++i)
00223   {
00224     Eigen::Vector4f pt (planar_hull_->points[i].x, planar_hull_->points[i].y, planar_hull_->points[i].z, 0);
00225     polygon.points[i].x = pt[k1];
00226     polygon.points[i].y = pt[k2];
00227     polygon.points[i].z = 0;
00228   }
00229 
00230   PointT pt_xy;
00231   pt_xy.z = 0;
00232 
00233   output.indices.resize (indices_->size ());
00234   int l = 0;
00235   for (size_t i = 0; i < projected_points.points.size (); ++i)
00236   {
00237     // Check the distance to the user imposed limits from the table planar model
00238     double distance = pointToPlaneDistanceSigned (input_->points[(*indices_)[i]], model_coefficients);
00239     if (distance < height_limit_min_ || distance > height_limit_max_)
00240       continue;
00241 
00242     // Check what points are inside the hull
00243     Eigen::Vector4f pt (projected_points.points[i].x,
00244                          projected_points.points[i].y,
00245                          projected_points.points[i].z, 0);
00246     pt_xy.x = pt[k1];
00247     pt_xy.y = pt[k2];
00248 
00249     if (!pcl::isXYPointIn2DXYPolygon (pt_xy, polygon))
00250       continue;
00251 
00252     output.indices[l++] = (*indices_)[i];
00253   }
00254   output.indices.resize (l);
00255 
00256   deinitCompute ();
00257 }
00258 
00259 #define PCL_INSTANTIATE_ExtractPolygonalPrismData(T) template class PCL_EXPORTS pcl::ExtractPolygonalPrismData<T>;
00260 #define PCL_INSTANTIATE_isPointIn2DPolygon(T) template bool PCL_EXPORTS pcl::isPointIn2DPolygon<T>(const T&, const pcl::PointCloud<T> &);
00261 #define PCL_INSTANTIATE_isXYPointIn2DXYPolygon(T) template bool PCL_EXPORTS pcl::isXYPointIn2DXYPolygon<T>(const T &, const pcl::PointCloud<T> &);
00262 
00263 #endif    // PCL_SEGMENTATION_IMPL_EXTRACT_POLYGONAL_PRISM_DATA_H_
00264 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines