Point Cloud Library (PCL) 1.12.0
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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
44template<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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
69template<typename PointT> void
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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
94template<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 (pcl::Vertices const & poly : hull_polygons_)
108 {
109 for (auto const & idx : poly.vertices)
110 {
111 Eigen::Vector3f pt = (*hull_cloud_)[idx].getVector3fMap ();
112 cloud_min = cloud_min.cwiseMin(pt);
113 cloud_max = cloud_max.cwiseMax(pt);
114 }
115 }
116
117 return (cloud_max - cloud_min);
118}
119
120//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
121template<typename PointT> template<unsigned PlaneDim1, unsigned PlaneDim2> void
123{
124 for (std::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 std::size_t poly;
129 for (poly = 0; poly < hull_polygons_.size (); poly++)
130 {
131 if (isPointIn2DPolyWithVertIndices<PlaneDim1,PlaneDim2> (
132 (*input_)[(*indices_)[index]], hull_polygons_[poly], *hull_cloud_
133 ))
134 {
135 if (crop_outside_)
136 output.push_back ((*input_)[(*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_)[(*indices_)[index]]);
146 }
147}
148
149//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
150template<typename PointT> template<unsigned PlaneDim1, unsigned PlaneDim2> void
152{
153 // see comments in (PointCloud& output) overload
154 for (std::size_t index = 0; index < indices_->size (); index++)
155 {
156 std::size_t poly;
157 for (poly = 0; poly < hull_polygons_.size (); poly++)
158 {
159 if (isPointIn2DPolyWithVertIndices<PlaneDim1,PlaneDim2> (
160 (*input_)[(*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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
174template<typename PointT> void
176{
177 // This algorithm could definitely be sped up using kdtree/octree
178 // information, if that is available!
179
180 for (std::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 std::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 (std::size_t poly = 0; poly < hull_polygons_.size (); poly++)
198 for (std::size_t ray = 0; ray < 3; ray++)
199 crossings[ray] += rayTriangleIntersect
200 ((*input_)[(*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_)[(*indices_)[index]]);
204 else if (!crop_outside_)
205 output.push_back ((*input_)[(*indices_)[index]]);
206 }
207}
208
209//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
210template<typename PointT> void
212{
213 // see comments in applyFilter3D (PointCloud& output)
214 for (std::size_t index = 0; index < indices_->size (); index++)
215 {
216 std::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 (std::size_t poly = 0; poly < hull_polygons_.size (); poly++)
225 for (std::size_t ray = 0; ray < 3; ray++)
226 crossings[ray] += rayTriangleIntersect
227 ((*input_)[(*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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
237template<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<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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
279template<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_
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:45
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:652
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