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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
45template<typename PointT>
46PCL_DEPRECATED(1, 13, "This is a trivial call to base class method")
47void
48pcl::CropHull<PointT>::applyFilter (PointCloud &output)
49{
51}
52
53
54//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
55template<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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
84template<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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
111template<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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
143template<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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
181template<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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
223template<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