Point Cloud Library (PCL)  1.9.1-dev
crop_hull.hpp
1  /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef PCL_FILTERS_IMPL_CROP_HULL_H_
39 #define PCL_FILTERS_IMPL_CROP_HULL_H_
40 
41 #include <pcl/filters/crop_hull.h>
42 
43 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
44 template<typename PointT> void
46 {
47  if (dim_ == 2)
48  {
49  // in this case we are assuming all the points lie in the same plane as the
50  // 2D convex hull, so the choice of projection just changes the
51  // conditioning of the problem: choose to squash the XYZ component of the
52  // hull-points that has least variation - this will also give reasonable
53  // results if the points don't lie exactly in the same plane
54  const Eigen::Vector3f range = getHullCloudRange ();
55  if (range[0] <= range[1] && range[0] <= range[2])
56  applyFilter2D<1,2> (output);
57  else if (range[1] <= range[2] && range[1] <= range[0])
58  applyFilter2D<2,0> (output);
59  else
60  applyFilter2D<0,1> (output);
61  }
62  else
63  {
64  applyFilter3D (output);
65  }
66 }
67 
68 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
69 template<typename PointT> void
70 pcl::CropHull<PointT>::applyFilter (std::vector<int> &indices)
71 {
72  if (dim_ == 2)
73  {
74  // in this case we are assuming all the points lie in the same plane as the
75  // 2D convex hull, so the choice of projection just changes the
76  // conditioning of the problem: choose to squash the XYZ component of the
77  // hull-points that has least variation - this will also give reasonable
78  // results if the points don't lie exactly in the same plane
79  const Eigen::Vector3f range = getHullCloudRange ();
80  if (range[0] <= range[1] && range[0] <= range[2])
81  applyFilter2D<1,2> (indices);
82  else if (range[1] <= range[2] && range[1] <= range[0])
83  applyFilter2D<2,0> (indices);
84  else
85  applyFilter2D<0,1> (indices);
86  }
87  else
88  {
89  applyFilter3D (indices);
90  }
91 }
92 
93 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
94 template<typename PointT> Eigen::Vector3f
96 {
97  Eigen::Vector3f cloud_min (
98  std::numeric_limits<float>::max (),
99  std::numeric_limits<float>::max (),
100  std::numeric_limits<float>::max ()
101  );
102  Eigen::Vector3f cloud_max (
103  -std::numeric_limits<float>::max (),
104  -std::numeric_limits<float>::max (),
105  -std::numeric_limits<float>::max ()
106  );
107  for (size_t index = 0; index < indices_->size (); index++)
108  {
109  Eigen::Vector3f pt = input_->points[(*indices_)[index]].getVector3fMap ();
110  for (int i = 0; i < 3; i++)
111  {
112  if (pt[i] < cloud_min[i]) cloud_min[i] = pt[i];
113  if (pt[i] > cloud_max[i]) cloud_max[i] = pt[i];
114  }
115  }
116 
117  return (cloud_max - cloud_min);
118 }
119 
120 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
121 template<typename PointT> template<unsigned PlaneDim1, unsigned PlaneDim2> void
123 {
124  for (size_t index = 0; index < indices_->size (); index++)
125  {
126  // iterate over polygons faster than points because we expect this data
127  // to be, in general, more cache-local - the point cloud might be huge
128  size_t poly;
129  for (poly = 0; poly < hull_polygons_.size (); poly++)
130  {
131  if (isPointIn2DPolyWithVertIndices<PlaneDim1,PlaneDim2> (
132  input_->points[(*indices_)[index]], hull_polygons_[poly], *hull_cloud_
133  ))
134  {
135  if (crop_outside_)
136  output.push_back (input_->points[(*indices_)[index]]);
137  // once a point has tested +ve for being inside one polygon, we can
138  // stop checking the others:
139  break;
140  }
141  }
142  // If we're removing points *inside* the hull, only remove points that
143  // haven't been found inside any polygons
144  if (poly == hull_polygons_.size () && !crop_outside_)
145  output.push_back (input_->points[(*indices_)[index]]);
146  }
147 }
148 
149 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
150 template<typename PointT> template<unsigned PlaneDim1, unsigned PlaneDim2> void
151 pcl::CropHull<PointT>::applyFilter2D (std::vector<int> &indices)
152 {
153  // see comments in (PointCloud& output) overload
154  for (size_t index = 0; index < indices_->size (); index++)
155  {
156  size_t poly;
157  for (poly = 0; poly < hull_polygons_.size (); poly++)
158  {
159  if (isPointIn2DPolyWithVertIndices<PlaneDim1,PlaneDim2> (
160  input_->points[(*indices_)[index]], hull_polygons_[poly], *hull_cloud_
161  ))
162  {
163  if (crop_outside_)
164  indices.push_back ((*indices_)[index]);
165  break;
166  }
167  }
168  if (poly == hull_polygons_.size () && !crop_outside_)
169  indices.push_back ((*indices_)[index]);
170  }
171 }
172 
173 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
174 template<typename PointT> void
176 {
177  // This algorithm could definitely be sped up using kdtree/octree
178  // information, if that is available!
179 
180  for (size_t index = 0; index < indices_->size (); index++)
181  {
182  // test ray-crossings for three random rays, and take vote of crossings
183  // counts to determine if each point is inside the hull: the vote avoids
184  // tricky edge and corner cases when rays might fluke through the edge
185  // between two polygons
186  // 'random' rays are arbitrary - basically anything that is less likely to
187  // hit the edge between polygons than coordinate-axis aligned rays would
188  // be.
189  size_t crossings[3] = {0,0,0};
190  Eigen::Vector3f rays[3] =
191  {
192  Eigen::Vector3f (0.264882f, 0.688399f, 0.675237f),
193  Eigen::Vector3f (0.0145419f, 0.732901f, 0.68018f),
194  Eigen::Vector3f (0.856514f, 0.508771f, 0.0868081f)
195  };
196 
197  for (size_t poly = 0; poly < hull_polygons_.size (); poly++)
198  for (size_t ray = 0; ray < 3; ray++)
199  crossings[ray] += rayTriangleIntersect
200  (input_->points[(*indices_)[index]], rays[ray], hull_polygons_[poly], *hull_cloud_);
201 
202  if (crop_outside_ && (crossings[0]&1) + (crossings[1]&1) + (crossings[2]&1) > 1)
203  output.push_back (input_->points[(*indices_)[index]]);
204  else if (!crop_outside_)
205  output.push_back (input_->points[(*indices_)[index]]);
206  }
207 }
208 
209 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
210 template<typename PointT> void
211 pcl::CropHull<PointT>::applyFilter3D (std::vector<int> &indices)
212 {
213  // see comments in applyFilter3D (PointCloud& output)
214  for (size_t index = 0; index < indices_->size (); index++)
215  {
216  size_t crossings[3] = {0,0,0};
217  Eigen::Vector3f rays[3] =
218  {
219  Eigen::Vector3f(0.264882f, 0.688399f, 0.675237f),
220  Eigen::Vector3f(0.0145419f, 0.732901f, 0.68018f),
221  Eigen::Vector3f(0.856514f, 0.508771f, 0.0868081f)
222  };
223 
224  for (size_t poly = 0; poly < hull_polygons_.size (); poly++)
225  for (size_t ray = 0; ray < 3; ray++)
226  crossings[ray] += rayTriangleIntersect
227  (input_->points[(*indices_)[index]], rays[ray], hull_polygons_[poly], *hull_cloud_);
228 
229  if (crop_outside_ && (crossings[0]&1) + (crossings[1]&1) + (crossings[2]&1) > 1)
230  indices.push_back ((*indices_)[index]);
231  else if (!crop_outside_)
232  indices.push_back ((*indices_)[index]);
233  }
234 }
235 
236 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
237 template<typename PointT> template<unsigned PlaneDim1, unsigned PlaneDim2> bool
239  const PointT& point, const Vertices& verts, const PointCloud& cloud)
240 {
241  bool in_poly = false;
242  double x1, x2, y1, y2;
243 
244  const int nr_poly_points = static_cast<const int>(verts.vertices.size ());
245  double xold = cloud[verts.vertices[nr_poly_points - 1]].getVector3fMap ()[PlaneDim1];
246  double yold = cloud[verts.vertices[nr_poly_points - 1]].getVector3fMap ()[PlaneDim2];
247  for (int i = 0; i < nr_poly_points; i++)
248  {
249  const double xnew = cloud[verts.vertices[i]].getVector3fMap ()[PlaneDim1];
250  const double ynew = cloud[verts.vertices[i]].getVector3fMap ()[PlaneDim2];
251  if (xnew > xold)
252  {
253  x1 = xold;
254  x2 = xnew;
255  y1 = yold;
256  y2 = ynew;
257  }
258  else
259  {
260  x1 = xnew;
261  x2 = xold;
262  y1 = ynew;
263  y2 = yold;
264  }
265 
266  if ((xnew < point.getVector3fMap ()[PlaneDim1]) == (point.getVector3fMap ()[PlaneDim1] <= xold) &&
267  (point.getVector3fMap ()[PlaneDim2] - y1) * (x2 - x1) < (y2 - y1) * (point.getVector3fMap ()[PlaneDim1] - x1))
268  {
269  in_poly = !in_poly;
270  }
271  xold = xnew;
272  yold = ynew;
273  }
274 
275  return (in_poly);
276 }
277 
278 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
279 template<typename PointT> bool
281  const Eigen::Vector3f& ray,
282  const Vertices& verts,
283  const PointCloud& cloud)
284 {
285  // Algorithm here is adapted from:
286  // http://softsurfer.com/Archive/algorithm_0105/algorithm_0105.htm#intersect_RayTriangle()
287  //
288  // Original copyright notice:
289  // Copyright 2001, softSurfer (www.softsurfer.com)
290  // This code may be freely used and modified for any purpose
291  // providing that this copyright notice is included with it.
292  //
293  assert (verts.vertices.size () == 3);
294 
295  const Eigen::Vector3f p = point.getVector3fMap ();
296  const Eigen::Vector3f a = cloud[verts.vertices[0]].getVector3fMap ();
297  const Eigen::Vector3f b = cloud[verts.vertices[1]].getVector3fMap ();
298  const Eigen::Vector3f c = cloud[verts.vertices[2]].getVector3fMap ();
299  const Eigen::Vector3f u = b - a;
300  const Eigen::Vector3f v = c - a;
301  const Eigen::Vector3f n = u.cross (v);
302  const float n_dot_ray = n.dot (ray);
303 
304  if (std::fabs (n_dot_ray) < 1e-9)
305  return (false);
306 
307  const float r = n.dot (a - p) / n_dot_ray;
308 
309  if (r < 0)
310  return (false);
311 
312  const Eigen::Vector3f w = p + r * ray - a;
313  const float denominator = u.dot (v) * u.dot (v) - u.dot (u) * v.dot (v);
314  const float s_numerator = u.dot (v) * w.dot (v) - v.dot (v) * w.dot (u);
315  const float s = s_numerator / denominator;
316  if (s < 0 || s > 1)
317  return (false);
318 
319  const float t_numerator = u.dot (v) * w.dot (u) - u.dot (u) * w.dot (v);
320  const float t = t_numerator / denominator;
321  if (t < 0 || s+t > 1)
322  return (false);
323 
324  return (true);
325 }
326 
327 #define PCL_INSTANTIATE_CropHull(T) template class PCL_EXPORTS pcl::CropHull<T>;
328 
329 #endif // PCL_FILTERS_IMPL_CROP_HULL_H_
std::vector< uint32_t > vertices
Definition: Vertices.h:19
Filter points that lie inside or outside a 3D closed surface or 2D closed polygon, as generated by the ConvexHull or ConcaveHull classes.
Definition: crop_hull.h:52
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:479
void applyFilter(PointCloud &output) override
Filter the input points using the 2D or 3D polygon hull.
Definition: crop_hull.hpp:45
Describes a set of vertices in a polygon mesh, by basically storing an array of indices.
Definition: Vertices.h:14
PointCloud represents the base class in PCL for storing collections of 3D points. ...
A point structure representing Euclidean xyz coordinates, and the RGB color.