Point Cloud Library (PCL)  1.12.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>
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  bool crosses = (crossings[0]&1) + (crossings[1]&1) + (crossings[2]&1) > 1;
172  if ((crop_outside_ && crosses) || (!crop_outside_ && !crosses))
173  indices.push_back ((*indices_)[index]);
174  else
175  removed_indices_->push_back ((*indices_)[index]);
176  }
177 }
178 
179 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
180 template<typename PointT> template<unsigned PlaneDim1, unsigned PlaneDim2> bool
182  const PointT& point, const Vertices& verts, const PointCloud& cloud)
183 {
184  bool in_poly = false;
185  double x1, x2, y1, y2;
186 
187  const int nr_poly_points = static_cast<int>(verts.vertices.size ());
188  double xold = cloud[verts.vertices[nr_poly_points - 1]].getVector3fMap ()[PlaneDim1];
189  double yold = cloud[verts.vertices[nr_poly_points - 1]].getVector3fMap ()[PlaneDim2];
190  for (int i = 0; i < nr_poly_points; i++)
191  {
192  const double xnew = cloud[verts.vertices[i]].getVector3fMap ()[PlaneDim1];
193  const double ynew = cloud[verts.vertices[i]].getVector3fMap ()[PlaneDim2];
194  if (xnew > xold)
195  {
196  x1 = xold;
197  x2 = xnew;
198  y1 = yold;
199  y2 = ynew;
200  }
201  else
202  {
203  x1 = xnew;
204  x2 = xold;
205  y1 = ynew;
206  y2 = yold;
207  }
208 
209  if ((xnew < point.getVector3fMap ()[PlaneDim1]) == (point.getVector3fMap ()[PlaneDim1] <= xold) &&
210  (point.getVector3fMap ()[PlaneDim2] - y1) * (x2 - x1) < (y2 - y1) * (point.getVector3fMap ()[PlaneDim1] - x1))
211  {
212  in_poly = !in_poly;
213  }
214  xold = xnew;
215  yold = ynew;
216  }
217 
218  return (in_poly);
219 }
220 
221 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
222 template<typename PointT> bool
224  const Eigen::Vector3f& ray,
225  const Vertices& verts,
226  const PointCloud& cloud)
227 {
228  // Algorithm here is adapted from:
229  // http://softsurfer.com/Archive/algorithm_0105/algorithm_0105.htm#intersect_RayTriangle()
230  //
231  // Original copyright notice:
232  // Copyright 2001, softSurfer (www.softsurfer.com)
233  // This code may be freely used and modified for any purpose
234  // providing that this copyright notice is included with it.
235  //
236  assert (verts.vertices.size () == 3);
237 
238  const Eigen::Vector3f p = point.getVector3fMap ();
239  const Eigen::Vector3f a = cloud[verts.vertices[0]].getVector3fMap ();
240  const Eigen::Vector3f b = cloud[verts.vertices[1]].getVector3fMap ();
241  const Eigen::Vector3f c = cloud[verts.vertices[2]].getVector3fMap ();
242  const Eigen::Vector3f u = b - a;
243  const Eigen::Vector3f v = c - a;
244  const Eigen::Vector3f n = u.cross (v);
245  const float n_dot_ray = n.dot (ray);
246 
247  if (std::fabs (n_dot_ray) < 1e-9)
248  return (false);
249 
250  const float r = n.dot (a - p) / n_dot_ray;
251 
252  if (r < 0)
253  return (false);
254 
255  const Eigen::Vector3f w = p + r * ray - a;
256  const float denominator = u.dot (v) * u.dot (v) - u.dot (u) * v.dot (v);
257  const float s_numerator = u.dot (v) * w.dot (v) - v.dot (v) * w.dot (u);
258  const float s = s_numerator / denominator;
259  if (s < 0 || s > 1)
260  return (false);
261 
262  const float t_numerator = u.dot (v) * w.dot (u) - u.dot (u) * w.dot (v);
263  const float t = t_numerator / denominator;
264  if (t < 0 || s+t > 1)
265  return (false);
266 
267  return (true);
268 }
269 
270 #define PCL_INSTANTIATE_CropHull(T) template class PCL_EXPORTS pcl::CropHull<T>;
271 
272 #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