crop_hull.hpp
Go to the documentation of this file.
00001  /*
00002   * Software License Agreement (BSD License)
00003   *
00004   *  Point Cloud Library (PCL) - www.pointclouds.org
00005   *  Copyright (c) 2011, Willow Garage, Inc.
00006   *
00007   *  All rights reserved.
00008   *
00009   *  Redistribution and use in source and binary forms, with or without
00010   *  modification, are permitted provided that the following conditions
00011   *  are met:
00012   *
00013   *   * Redistributions of source code must retain the above copyright
00014   *     notice, this list of conditions and the following disclaimer.
00015   *   * Redistributions in binary form must reproduce the above
00016   *     copyright notice, this list of conditions and the following
00017   *     disclaimer in the documentation and/or other materials provided
00018   *     with the distribution.
00019   *   * Neither the name of Willow Garage, Inc. nor the names of its
00020   *     contributors may be used to endorse or promote products derived
00021   *     from this software without specific prior written permission.
00022   *
00023   *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024   *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025   *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026   *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027   *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028   *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029   *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030   *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031   *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032   *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033   *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034   *  POSSIBILITY OF SUCH DAMAGE.
00035   *
00036   */
00037 
00038 #ifndef PCL_FILTERS_IMPL_CROP_HULL_H_
00039 #define PCL_FILTERS_IMPL_CROP_HULL_H_
00040 
00041 #include <pcl/filters/crop_hull.h>
00042 
00044 template<typename PointT> void
00045 pcl::CropHull<PointT>::applyFilter (PointCloud &output)
00046 {
00047   if (dim_ == 2)
00048   {
00049     // in this case we are assuming all the points lie in the same plane as the
00050     // 2D convex hull, so the choice of projection just changes the
00051     // conditioning of the problem: choose to squash the XYZ component of the
00052     // hull-points that has least variation - this will also give reasonable
00053     // results if the points don't lie exactly in the same plane
00054     const Eigen::Vector3f range = getHullCloudRange ();
00055     if (range[0] <= range[1] && range[0] <= range[2])
00056       applyFilter2D<1,2> (output);
00057     else if (range[1] <= range[2] && range[1] <= range[0])
00058       applyFilter2D<2,0> (output);
00059     else
00060       applyFilter2D<0,1> (output);
00061   }
00062   else
00063   {
00064     applyFilter3D (output);
00065   }
00066 }
00067 
00069 template<typename PointT> void
00070 pcl::CropHull<PointT>::applyFilter (std::vector<int> &indices)
00071 {
00072   if (dim_ == 2)
00073   {
00074     // in this case we are assuming all the points lie in the same plane as the
00075     // 2D convex hull, so the choice of projection just changes the
00076     // conditioning of the problem: choose to squash the XYZ component of the
00077     // hull-points that has least variation - this will also give reasonable
00078     // results if the points don't lie exactly in the same plane
00079     const Eigen::Vector3f range = getHullCloudRange ();
00080     if (range[0] <= range[1] && range[0] <= range[2])
00081       applyFilter2D<1,2> (indices);
00082     else if (range[1] <= range[2] && range[1] <= range[0])
00083       applyFilter2D<2,0> (indices);
00084     else
00085       applyFilter2D<0,1> (indices);
00086   }
00087   else
00088   {
00089     applyFilter3D (indices);
00090   }
00091 }
00092 
00094 template<typename PointT> Eigen::Vector3f
00095 pcl::CropHull<PointT>::getHullCloudRange ()
00096 {
00097   Eigen::Vector3f cloud_min (
00098     std::numeric_limits<float> ().max (),
00099     std::numeric_limits<float> ().max (),
00100     std::numeric_limits<float> ().max ()
00101   );
00102   Eigen::Vector3f cloud_max (
00103     -std::numeric_limits<float> ().max (),
00104     -std::numeric_limits<float> ().max (),
00105     -std::numeric_limits<float> ().max ()
00106   );
00107   for (size_t index = 0; index < indices_->size (); index++)
00108   {
00109     Eigen::Vector3f pt = input_->points[(*indices_)[index]].getVector3fMap ();
00110     for (int i = 0; i < 3; i++)
00111     {
00112       if (pt[i] < cloud_min[i]) cloud_min[i] = pt[i];
00113       if (pt[i] > cloud_max[i]) cloud_max[i] = pt[i];
00114     }
00115   }
00116   
00117   return (cloud_max - cloud_min);
00118 }
00119 
00121 template<typename PointT> template<unsigned PlaneDim1, unsigned PlaneDim2> void 
00122 pcl::CropHull<PointT>::applyFilter2D (PointCloud &output)
00123 {
00124   for (size_t index = 0; index < indices_->size (); index++)
00125   {
00126     // iterate over polygons faster than points because we expect this data
00127     // to be, in general, more cache-local - the point cloud might be huge
00128     size_t poly;
00129     for (poly = 0; poly < hull_polygons_.size (); poly++)
00130     {
00131       if (isPointIn2DPolyWithVertIndices<PlaneDim1,PlaneDim2> (
00132               input_->points[(*indices_)[index]], hull_polygons_[poly], *hull_cloud_
00133          ))
00134       {
00135         if (crop_outside_)
00136           output.push_back (input_->points[(*indices_)[index]]);
00137         // once a point has tested +ve for being inside one polygon, we can
00138         // stop checking the others:
00139         break;
00140       }
00141     }
00142     // If we're removing points *inside* the hull, only remove points that
00143     // haven't been found inside any polygons
00144     if (poly == hull_polygons_.size () && !crop_outside_)
00145       output.push_back (input_->points[(*indices_)[index]]);
00146   }
00147 }
00148 
00150 template<typename PointT> template<unsigned PlaneDim1, unsigned PlaneDim2> void 
00151 pcl::CropHull<PointT>::applyFilter2D (std::vector<int> &indices)
00152 {
00153   // see comments in (PointCloud& output) overload
00154   for (size_t index = 0; index < indices_->size (); index++)
00155   {
00156     size_t poly;
00157     for (poly = 0; poly < hull_polygons_.size (); poly++)
00158     {
00159       if (isPointIn2DPolyWithVertIndices<PlaneDim1,PlaneDim2> (
00160               input_->points[(*indices_)[index]], hull_polygons_[poly], *hull_cloud_
00161          ))
00162       {
00163         if (crop_outside_)      
00164           indices.push_back ((*indices_)[index]);
00165         break;
00166       }
00167     }
00168     if (poly == hull_polygons_.size () && !crop_outside_)
00169       indices.push_back ((*indices_)[index]);
00170   }
00171 }
00172 
00174 template<typename PointT> void 
00175 pcl::CropHull<PointT>::applyFilter3D (PointCloud &output)
00176 {
00177   // This algorithm could definitely be sped up using kdtree/octree
00178   // information, if that is available!
00179 
00180   for (size_t index = 0; index < indices_->size (); index++)
00181   {
00182     // test ray-crossings for three random rays, and take vote of crossings
00183     // counts to determine if each point is inside the hull: the vote avoids
00184     // tricky edge and corner cases when rays might fluke through the edge
00185     // between two polygons
00186     // 'random' rays are arbitrary - basically anything that is less likely to
00187     // hit the edge between polygons than coordinate-axis aligned rays would
00188     // be.
00189     size_t crossings[3] = {0,0,0};
00190     Eigen::Vector3f rays[3] = 
00191     {
00192       Eigen::Vector3f (0.264882f,  0.688399f, 0.675237f),
00193       Eigen::Vector3f (0.0145419f, 0.732901f, 0.68018f),
00194       Eigen::Vector3f (0.856514f,  0.508771f, 0.0868081f)
00195     };
00196 
00197     for (size_t poly = 0; poly < hull_polygons_.size (); poly++)
00198       for (size_t ray = 0; ray < 3; ray++)
00199         crossings[ray] += rayTriangleIntersect
00200           (input_->points[(*indices_)[index]], rays[ray], hull_polygons_[poly], *hull_cloud_);
00201 
00202     if (crop_outside_ && (crossings[0]&1) + (crossings[1]&1) + (crossings[2]&1) > 1)
00203       output.push_back (input_->points[(*indices_)[index]]);
00204     else if (!crop_outside_)
00205       output.push_back (input_->points[(*indices_)[index]]);
00206   }
00207 }
00208 
00210 template<typename PointT> void 
00211 pcl::CropHull<PointT>::applyFilter3D (std::vector<int> &indices)
00212 {
00213   // see comments in applyFilter3D (PointCloud& output)
00214   for (size_t index = 0; index < indices_->size (); index++)
00215   {
00216     size_t crossings[3] = {0,0,0};
00217     Eigen::Vector3f rays[3] = 
00218     {
00219       Eigen::Vector3f(0.264882f,  0.688399f, 0.675237f),
00220       Eigen::Vector3f(0.0145419f, 0.732901f, 0.68018f),
00221       Eigen::Vector3f(0.856514f,  0.508771f, 0.0868081f)
00222     };
00223 
00224     for (size_t poly = 0; poly < hull_polygons_.size (); poly++)
00225       for (size_t ray = 0; ray < 3; ray++)
00226         crossings[ray] += rayTriangleIntersect
00227           (input_->points[(*indices_)[index]], rays[ray], hull_polygons_[poly], *hull_cloud_);
00228 
00229     if (crop_outside_ && (crossings[0]&1) + (crossings[1]&1) + (crossings[2]&1) > 1)
00230       indices.push_back ((*indices_)[index]);
00231     else if (!crop_outside_)
00232       indices.push_back ((*indices_)[index]);
00233   }
00234 }
00235 
00237 template<typename PointT> template<unsigned PlaneDim1, unsigned PlaneDim2> bool
00238 pcl::CropHull<PointT>::isPointIn2DPolyWithVertIndices (
00239   const PointT& point, const Vertices& verts, const PointCloud& cloud)
00240 {
00241   bool in_poly = false;
00242   double x1, x2, y1, y2;
00243 
00244   const int nr_poly_points = static_cast<const int>(verts.vertices.size ());
00245   double xold = cloud[verts.vertices[nr_poly_points - 1]].getVector3fMap ()[PlaneDim1];
00246   double yold = cloud[verts.vertices[nr_poly_points - 1]].getVector3fMap ()[PlaneDim2];
00247   for (int i = 0; i < nr_poly_points; i++)
00248   {
00249     const double xnew = cloud[verts.vertices[i]].getVector3fMap ()[PlaneDim1];
00250     const double ynew = cloud[verts.vertices[i]].getVector3fMap ()[PlaneDim2];
00251     if (xnew > xold)
00252     {
00253       x1 = xold;
00254       x2 = xnew;
00255       y1 = yold;
00256       y2 = ynew;
00257     }
00258     else
00259     {
00260       x1 = xnew;
00261       x2 = xold;
00262       y1 = ynew;
00263       y2 = yold;
00264     }
00265 
00266     if ((xnew < point.getVector3fMap ()[PlaneDim1]) == (point.getVector3fMap ()[PlaneDim1] <= xold) &&
00267         (point.getVector3fMap ()[PlaneDim2] - y1) * (x2 - x1) < (y2 - y1) * (point.getVector3fMap ()[PlaneDim1] - x1))
00268     {
00269       in_poly = !in_poly;
00270     }
00271     xold = xnew;
00272     yold = ynew;
00273   }
00274 
00275   return (in_poly);
00276 }
00277 
00279 template<typename PointT> bool
00280 pcl::CropHull<PointT>::rayTriangleIntersect (const PointT& point,
00281                                              const Eigen::Vector3f& ray,
00282                                              const Vertices& verts,
00283                                              const PointCloud& cloud)
00284 {
00285   // Algorithm here is adapted from:
00286   // http://softsurfer.com/Archive/algorithm_0105/algorithm_0105.htm#intersect_RayTriangle()
00287   //
00288   // Original copyright notice:
00289   // Copyright 2001, softSurfer (www.softsurfer.com)
00290   // This code may be freely used and modified for any purpose
00291   // providing that this copyright notice is included with it.
00292   //
00293   assert (verts.vertices.size () == 3);
00294 
00295   const Eigen::Vector3f p = point.getVector3fMap ();
00296   const Eigen::Vector3f a = cloud[verts.vertices[0]].getVector3fMap ();
00297   const Eigen::Vector3f b = cloud[verts.vertices[1]].getVector3fMap ();
00298   const Eigen::Vector3f c = cloud[verts.vertices[2]].getVector3fMap ();
00299   const Eigen::Vector3f u = b - a;
00300   const Eigen::Vector3f v = c - a;
00301   const Eigen::Vector3f n = u.cross (v);
00302   const float n_dot_ray = n.dot (ray);
00303 
00304   if (std::fabs (n_dot_ray) < 1e-9)
00305     return (false);
00306 
00307   const float r = n.dot (a - p) / n_dot_ray;
00308 
00309   if (r < 0)
00310     return (false);
00311 
00312   const Eigen::Vector3f w = p + r * ray - a;
00313   const float denominator = u.dot (v) * u.dot (v) - u.dot (u) * v.dot (v);
00314   const float s_numerator = u.dot (v) * w.dot (v) - v.dot (v) * w.dot (u);
00315   const float s = s_numerator / denominator;
00316   if (s < 0 || s > 1)
00317     return (false);
00318 
00319   const float t_numerator = u.dot (v) * w.dot (u) - u.dot (u) * w.dot (v);
00320   const float t = t_numerator / denominator;
00321   if (t < 0 || s+t > 1)
00322     return (false);
00323   
00324   return (true);
00325 }
00326 
00327 #define PCL_INSTANTIATE_CropHull(T) template class PCL_EXPORTS pcl::CropHull<T>;
00328 
00329 #endif // PCL_FILTERS_IMPL_CROP_HULL_H_


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:14:48