Point Cloud Library (PCL)
1.7.0
|
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 the copyright holder(s) 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 00043 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 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 00068 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 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 00093 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 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 00120 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 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 00149 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 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 00173 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 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 00209 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 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 00236 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 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 00278 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 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_