Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
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> 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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
74template<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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
101template<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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
133template<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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
170template<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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
212template<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.
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