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