Point Cloud Library (PCL)  1.12.1
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 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
45 template<typename PointT>
46 PCL_DEPRECATED(1, 13, "This is a trivial call to base class method")
47 void
48 pcl::CropHull<PointT>::applyFilter (PointCloud &output)
49 {
51 }
52 
53 
54 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
55 template<typename PointT> void
57 {
58  indices.clear();
59  removed_indices_->clear();
60  indices.reserve(indices_->size());
61  removed_indices_->reserve(indices_->size());
62  if (dim_ == 2)
63  {
64  // in this case we are assuming all the points lie in the same plane as the
65  // 2D convex hull, so the choice of projection just changes the
66  // conditioning of the problem: choose to squash the XYZ component of the
67  // hull-points that has least variation - this will also give reasonable
68  // results if the points don't lie exactly in the same plane
69  const Eigen::Vector3f range = getHullCloudRange ();
70  if (range[0] <= range[1] && range[0] <= range[2])
71  applyFilter2D<1,2> (indices);
72  else if (range[1] <= range[2] && range[1] <= range[0])
73  applyFilter2D<2,0> (indices);
74  else
75  applyFilter2D<0,1> (indices);
76  }
77  else
78  {
79  applyFilter3D (indices);
80  }
81 }
82 
83 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
84 template<typename PointT> Eigen::Vector3f
86 {
87  Eigen::Vector3f cloud_min (
88  std::numeric_limits<float>::max (),
89  std::numeric_limits<float>::max (),
90  std::numeric_limits<float>::max ()
91  );
92  Eigen::Vector3f cloud_max (
93  -std::numeric_limits<float>::max (),
94  -std::numeric_limits<float>::max (),
95  -std::numeric_limits<float>::max ()
96  );
97  for (pcl::Vertices const & poly : hull_polygons_)
98  {
99  for (auto const & idx : poly.vertices)
100  {
101  Eigen::Vector3f pt = (*hull_cloud_)[idx].getVector3fMap ();
102  cloud_min = cloud_min.cwiseMin(pt);
103  cloud_max = cloud_max.cwiseMax(pt);
104  }
105  }
106 
107  return (cloud_max - cloud_min);
108 }
109 
110 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
111 template<typename PointT> template<unsigned PlaneDim1, unsigned PlaneDim2> void
113 {
114  for (std::size_t index = 0; index < indices_->size (); index++)
115  {
116  // iterate over polygons faster than points because we expect this data
117  // to be, in general, more cache-local - the point cloud might be huge
118  std::size_t poly;
119  for (poly = 0; poly < hull_polygons_.size (); poly++)
120  {
121  if (isPointIn2DPolyWithVertIndices<PlaneDim1,PlaneDim2> (
122  (*input_)[(*indices_)[index]], hull_polygons_[poly], *hull_cloud_
123  ))
124  {
125  if (crop_outside_)
126  indices.push_back ((*indices_)[index]);
127  // once a point has tested +ve for being inside one polygon, we can
128  // stop checking the others:
129  break;
130  }
131  }
132  // If we're removing points *inside* the hull, only remove points that
133  // haven't been found inside any polygons
134  if (poly == hull_polygons_.size () && !crop_outside_)
135  indices.push_back ((*indices_)[index]);
136  if (indices.empty() || indices.back() != (*indices_)[index]) {
137  removed_indices_->push_back ((*indices_)[index]);
138  }
139  }
140 }
141 
142 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
143 template<typename PointT> void
145 {
146  // This algorithm could definitely be sped up using kdtree/octree
147  // information, if that is available!
148 
149  for (std::size_t index = 0; index < indices_->size (); index++)
150  {
151  // test ray-crossings for three random rays, and take vote of crossings
152  // counts to determine if each point is inside the hull: the vote avoids
153  // tricky edge and corner cases when rays might fluke through the edge
154  // between two polygons
155  // 'random' rays are arbitrary - basically anything that is less likely to
156  // hit the edge between polygons than coordinate-axis aligned rays would
157  // be.
158  std::size_t crossings[3] = {0,0,0};
159  Eigen::Vector3f rays[3] =
160  {
161  Eigen::Vector3f(0.264882f, 0.688399f, 0.675237f),
162  Eigen::Vector3f(0.0145419f, 0.732901f, 0.68018f),
163  Eigen::Vector3f(0.856514f, 0.508771f, 0.0868081f)
164  };
165 
166  for (std::size_t poly = 0; poly < hull_polygons_.size (); poly++)
167  for (std::size_t ray = 0; ray < 3; ray++)
168  crossings[ray] += rayTriangleIntersect
169  ((*input_)[(*indices_)[index]], rays[ray], hull_polygons_[poly], *hull_cloud_);
170 
171  if (crop_outside_ && (crossings[0]&1) + (crossings[1]&1) + (crossings[2]&1) > 1)
172  indices.push_back ((*indices_)[index]);
173  else if (!crop_outside_)
174  indices.push_back ((*indices_)[index]);
175  else
176  removed_indices_->push_back ((*indices_)[index]);
177  }
178 }
179 
180 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
181 template<typename PointT> template<unsigned PlaneDim1, unsigned PlaneDim2> bool
183  const PointT& point, const Vertices& verts, const PointCloud& cloud)
184 {
185  bool in_poly = false;
186  double x1, x2, y1, y2;
187 
188  const int nr_poly_points = static_cast<int>(verts.vertices.size ());
189  double xold = cloud[verts.vertices[nr_poly_points - 1]].getVector3fMap ()[PlaneDim1];
190  double yold = cloud[verts.vertices[nr_poly_points - 1]].getVector3fMap ()[PlaneDim2];
191  for (int i = 0; i < nr_poly_points; i++)
192  {
193  const double xnew = cloud[verts.vertices[i]].getVector3fMap ()[PlaneDim1];
194  const double ynew = cloud[verts.vertices[i]].getVector3fMap ()[PlaneDim2];
195  if (xnew > xold)
196  {
197  x1 = xold;
198  x2 = xnew;
199  y1 = yold;
200  y2 = ynew;
201  }
202  else
203  {
204  x1 = xnew;
205  x2 = xold;
206  y1 = ynew;
207  y2 = yold;
208  }
209 
210  if ((xnew < point.getVector3fMap ()[PlaneDim1]) == (point.getVector3fMap ()[PlaneDim1] <= xold) &&
211  (point.getVector3fMap ()[PlaneDim2] - y1) * (x2 - x1) < (y2 - y1) * (point.getVector3fMap ()[PlaneDim1] - x1))
212  {
213  in_poly = !in_poly;
214  }
215  xold = xnew;
216  yold = ynew;
217  }
218 
219  return (in_poly);
220 }
221 
222 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
223 template<typename PointT> bool
225  const Eigen::Vector3f& ray,
226  const Vertices& verts,
227  const PointCloud& cloud)
228 {
229  // Algorithm here is adapted from:
230  // http://softsurfer.com/Archive/algorithm_0105/algorithm_0105.htm#intersect_RayTriangle()
231  //
232  // Original copyright notice:
233  // Copyright 2001, softSurfer (www.softsurfer.com)
234  // This code may be freely used and modified for any purpose
235  // providing that this copyright notice is included with it.
236  //
237  assert (verts.vertices.size () == 3);
238 
239  const Eigen::Vector3f p = point.getVector3fMap ();
240  const Eigen::Vector3f a = cloud[verts.vertices[0]].getVector3fMap ();
241  const Eigen::Vector3f b = cloud[verts.vertices[1]].getVector3fMap ();
242  const Eigen::Vector3f c = cloud[verts.vertices[2]].getVector3fMap ();
243  const Eigen::Vector3f u = b - a;
244  const Eigen::Vector3f v = c - a;
245  const Eigen::Vector3f n = u.cross (v);
246  const float n_dot_ray = n.dot (ray);
247 
248  if (std::fabs (n_dot_ray) < 1e-9)
249  return (false);
250 
251  const float r = n.dot (a - p) / n_dot_ray;
252 
253  if (r < 0)
254  return (false);
255 
256  const Eigen::Vector3f w = p + r * ray - a;
257  const float denominator = u.dot (v) * u.dot (v) - u.dot (u) * v.dot (v);
258  const float s_numerator = u.dot (v) * w.dot (v) - v.dot (v) * w.dot (u);
259  const float s = s_numerator / denominator;
260  if (s < 0 || s > 1)
261  return (false);
262 
263  const float t_numerator = u.dot (v) * w.dot (u) - u.dot (u) * w.dot (v);
264  const float t = t_numerator / denominator;
265  if (t < 0 || s+t > 1)
266  return (false);
267 
268  return (true);
269 }
270 
271 #define PCL_INSTANTIATE_CropHull(T) template class PCL_EXPORTS pcl::CropHull<T>;
272 
273 #endif // PCL_FILTERS_IMPL_CROP_HULL_H_
Filter points that lie inside or outside a 3D closed surface or 2D closed polygon,...
Definition: crop_hull.h:52
void applyFilter(PointCloud &output) override
Filter the input points using the 2D or 3D polygon hull.
Definition: crop_hull.hpp:48
FilterIndices represents the base class for filters that are about binary point removal.
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
#define PCL_DEPRECATED(Major, Minor, Message)
macro for compatibility across compilers and help remove old deprecated items for the Major....
Definition: pcl_macros.h:156
A point structure representing Euclidean xyz coordinates, and the RGB color.
Describes a set of vertices in a polygon mesh, by basically storing an array of indices.
Definition: Vertices.h:15