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 5026 2012-03-12 02:51:44Z rusu $
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   computeMeanAndCovarianceMatrix (polygon, covariance_matrix, xyz_centroid);
00055 
00056   // Compute the model coefficients
00057   EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
00058   EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
00059   eigen33 (covariance_matrix, eigen_value, eigen_vector);
00060 
00061   model_coefficients[0] = eigen_vector [0];
00062   model_coefficients[1] = eigen_vector [1];
00063   model_coefficients[2] = eigen_vector [2];
00064   model_coefficients[3] = 0;
00065 
00066   // Hessian form (D = nc . p_plane (centroid here) + p)
00067   model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
00068 
00069   float distance_to_plane = model_coefficients[0] * point.x +
00070                             model_coefficients[1] * point.y +
00071                             model_coefficients[2] * point.z +
00072                             model_coefficients[3];
00073   PointT ppoint;
00074   // Calculate the projection of the point on the plane
00075   ppoint.x = point.x - distance_to_plane * model_coefficients[0];
00076   ppoint.y = point.y - distance_to_plane * model_coefficients[1];
00077   ppoint.z = point.z - distance_to_plane * model_coefficients[2];
00078 
00079   // Create a X-Y projected representation for within bounds polygonal checking
00080   int k0, k1, k2;
00081   // Determine the best plane to project points onto
00082   k0 = (fabs (model_coefficients[0] ) > fabs (model_coefficients[1])) ? 0  : 1;
00083   k0 = (fabs (model_coefficients[k0]) > fabs (model_coefficients[2])) ? k0 : 2;
00084   k1 = (k0 + 1) % 3;
00085   k2 = (k0 + 2) % 3;
00086   // Project the convex hull
00087   pcl::PointCloud<PointT> xy_polygon;
00088   xy_polygon.points.resize (polygon.points.size ());
00089   for (size_t i = 0; i < polygon.points.size (); ++i)
00090   {
00091     Eigen::Vector4f pt (polygon.points[i].x, polygon.points[i].y, polygon.points[i].z, 0);
00092     xy_polygon.points[i].x = pt[k1];
00093     xy_polygon.points[i].y = pt[k2];
00094     xy_polygon.points[i].z = 0;
00095   }
00096   PointT xy_point;
00097   xy_point.z = 0;
00098   Eigen::Vector4f pt (ppoint.x, ppoint.y, ppoint.z, 0);
00099   xy_point.x = pt[k1];
00100   xy_point.y = pt[k2];
00101 
00102   return (pcl::isXYPointIn2DXYPolygon (xy_point, xy_polygon));
00103 }
00104 
00106 template <typename PointT> bool
00107 pcl::isXYPointIn2DXYPolygon (const PointT &point, const pcl::PointCloud<PointT> &polygon)
00108 {
00109   bool in_poly = false;
00110   double x1, x2, y1, y2;
00111 
00112   int nr_poly_points = static_cast<int> (polygon.points.size ());
00113   double xold = polygon.points[nr_poly_points - 1].x;
00114   double yold = polygon.points[nr_poly_points - 1].y;
00115   for (int i = 0; i < nr_poly_points; i++)
00116   {
00117     double xnew = polygon.points[i].x;
00118     double ynew = polygon.points[i].y;
00119     if (xnew > xold)
00120     {
00121       x1 = xold;
00122       x2 = xnew;
00123       y1 = yold;
00124       y2 = ynew;
00125     }
00126     else
00127     {
00128       x1 = xnew;
00129       x2 = xold;
00130       y1 = ynew;
00131       y2 = yold;
00132     }
00133 
00134     if ( (xnew < point.x) == (point.x <= xold) && (point.y - y1) * (x2 - x1) < (y2 - y1) * (point.x - x1) )
00135     {
00136       in_poly = !in_poly;
00137     }
00138     xold = xnew;
00139     yold = ynew;
00140   }
00141   return (in_poly);
00142 }
00143 
00145 template <typename PointT> void
00146 pcl::ExtractPolygonalPrismData<PointT>::segment (pcl::PointIndices &output)
00147 {
00148   output.header = input_->header;
00149 
00150   if (!initCompute ())
00151   {
00152     output.indices.clear ();
00153     return;
00154   }
00155 
00156   if (static_cast<int> (planar_hull_->points.size ()) < min_pts_hull_)
00157   {
00158     PCL_ERROR ("[pcl::%s::segment] Not enough points (%zu) in the hull!\n", getClassName ().c_str (), planar_hull_->points.size ());
00159     output.indices.clear ();
00160     return;
00161   }
00162 
00163   // Compute the plane coefficients
00164   Eigen::Vector4f model_coefficients;
00165   EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
00166   Eigen::Vector4f xyz_centroid;
00167 
00168   computeMeanAndCovarianceMatrix (*planar_hull_, covariance_matrix, xyz_centroid);
00169 
00170   // Compute the model coefficients
00171   EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
00172   EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
00173   eigen33 (covariance_matrix, eigen_value, eigen_vector);
00174 
00175   model_coefficients[0] = eigen_vector [0];
00176   model_coefficients[1] = eigen_vector [1];
00177   model_coefficients[2] = eigen_vector [2];
00178   model_coefficients[3] = 0;
00179 
00180   // Hessian form (D = nc . p_plane (centroid here) + p)
00181   model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
00182 
00183   // Need to flip the plane normal towards the viewpoint
00184   Eigen::Vector4f vp (vpx_, vpy_, vpz_, 0);
00185   // See if we need to flip any plane normals
00186   vp -= planar_hull_->points[0].getVector4fMap ();
00187   vp[3] = 0;
00188   // Dot product between the (viewpoint - point) and the plane normal
00189   float cos_theta = vp.dot (model_coefficients);
00190   // Flip the plane normal
00191   if (cos_theta < 0)
00192   {
00193     model_coefficients *= -1;
00194     model_coefficients[3] = 0;
00195     // Hessian form (D = nc . p_plane (centroid here) + p)
00196     model_coefficients[3] = -1 * (model_coefficients.dot (planar_hull_->points[0].getVector4fMap ()));
00197   }
00198 
00199   // Project all points
00200   PointCloud projected_points;
00201   SampleConsensusModelPlane<PointT> sacmodel (input_);
00202   sacmodel.projectPoints (*indices_, model_coefficients, projected_points, false);
00203 
00204   // Create a X-Y projected representation for within bounds polygonal checking
00205   int k0, k1, k2;
00206   // Determine the best plane to project points onto
00207   k0 = (fabs (model_coefficients[0] ) > fabs (model_coefficients[1])) ? 0  : 1;
00208   k0 = (fabs (model_coefficients[k0]) > fabs (model_coefficients[2])) ? k0 : 2;
00209   k1 = (k0 + 1) % 3;
00210   k2 = (k0 + 2) % 3;
00211   // Project the convex hull
00212   pcl::PointCloud<PointT> polygon;
00213   polygon.points.resize (planar_hull_->points.size ());
00214   for (size_t i = 0; i < planar_hull_->points.size (); ++i)
00215   {
00216     Eigen::Vector4f pt (planar_hull_->points[i].x, planar_hull_->points[i].y, planar_hull_->points[i].z, 0);
00217     polygon.points[i].x = pt[k1];
00218     polygon.points[i].y = pt[k2];
00219     polygon.points[i].z = 0;
00220   }
00221 
00222   PointT pt_xy;
00223   pt_xy.z = 0;
00224 
00225   output.indices.resize (indices_->size ());
00226   int l = 0;
00227   for (size_t i = 0; i < projected_points.points.size (); ++i)
00228   {
00229     // Check the distance to the user imposed limits from the table planar model
00230     double distance = pointToPlaneDistanceSigned (input_->points[(*indices_)[i]], model_coefficients);
00231     if (distance < height_limit_min_ || distance > height_limit_max_)
00232       continue;
00233 
00234     // Check what points are inside the hull
00235     Eigen::Vector4f pt (projected_points.points[i].x,
00236                          projected_points.points[i].y,
00237                          projected_points.points[i].z, 0);
00238     pt_xy.x = pt[k1];
00239     pt_xy.y = pt[k2];
00240 
00241     if (!pcl::isXYPointIn2DXYPolygon (pt_xy, polygon))
00242       continue;
00243 
00244     output.indices[l++] = (*indices_)[i];
00245   }
00246   output.indices.resize (l);
00247 
00248   deinitCompute ();
00249 }
00250 
00251 #define PCL_INSTANTIATE_ExtractPolygonalPrismData(T) template class PCL_EXPORTS pcl::ExtractPolygonalPrismData<T>;
00252 #define PCL_INSTANTIATE_isPointIn2DPolygon(T) template bool PCL_EXPORTS pcl::isPointIn2DPolygon<T>(const T&, const pcl::PointCloud<T> &);
00253 #define PCL_INSTANTIATE_isXYPointIn2DXYPolygon(T) template bool PCL_EXPORTS pcl::isXYPointIn2DXYPolygon<T>(const T &, const pcl::PointCloud<T> &);
00254 
00255 #endif    // PCL_SEGMENTATION_IMPL_EXTRACT_POLYGONAL_PRISM_DATA_H_
00256 


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:15:02