38 #ifndef PCL_FILTERS_IMPL_CROP_HULL_H_
39 #define PCL_FILTERS_IMPL_CROP_HULL_H_
41 #include <pcl/filters/crop_hull.h>
45 template<
typename Po
intT>
46 PCL_DEPRECATED(1, 13,
"This is a trivial call to base class method")
55 template<
typename Po
intT>
void
59 removed_indices_->clear();
60 indices.reserve(indices_->size());
61 removed_indices_->reserve(indices_->size());
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);
75 applyFilter2D<0,1> (indices);
79 applyFilter3D (indices);
84 template<
typename Po
intT> Eigen::Vector3f
87 Eigen::Vector3f cloud_min (
88 std::numeric_limits<float>::max (),
89 std::numeric_limits<float>::max (),
90 std::numeric_limits<float>::max ()
92 Eigen::Vector3f cloud_max (
93 -std::numeric_limits<float>::max (),
94 -std::numeric_limits<float>::max (),
95 -std::numeric_limits<float>::max ()
99 for (
auto const & idx : poly.vertices)
101 Eigen::Vector3f pt = (*hull_cloud_)[idx].getVector3fMap ();
102 cloud_min = cloud_min.cwiseMin(pt);
103 cloud_max = cloud_max.cwiseMax(pt);
107 return (cloud_max - cloud_min);
111 template<
typename Po
intT>
template<
unsigned PlaneDim1,
unsigned PlaneDim2>
void
114 for (std::size_t index = 0; index < indices_->size (); index++)
119 for (poly = 0; poly < hull_polygons_.size (); poly++)
121 if (isPointIn2DPolyWithVertIndices<PlaneDim1,PlaneDim2> (
122 (*input_)[(*indices_)[index]], hull_polygons_[poly], *hull_cloud_
126 indices.push_back ((*indices_)[index]);
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]);
143 template<
typename Po
intT>
void
149 for (std::size_t index = 0; index < indices_->size (); index++)
158 std::size_t crossings[3] = {0,0,0};
159 Eigen::Vector3f rays[3] =
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)
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_);
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]);
176 removed_indices_->push_back ((*indices_)[index]);
181 template<
typename Po
intT>
template<
unsigned PlaneDim1,
unsigned PlaneDim2>
bool
185 bool in_poly =
false;
186 double x1, x2, y1, y2;
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++)
193 const double xnew = cloud[verts.vertices[i]].getVector3fMap ()[PlaneDim1];
194 const double ynew = cloud[verts.vertices[i]].getVector3fMap ()[PlaneDim2];
210 if ((xnew < point.getVector3fMap ()[PlaneDim1]) == (point.getVector3fMap ()[PlaneDim1] <= xold) &&
211 (point.getVector3fMap ()[PlaneDim2] - y1) * (x2 - x1) < (y2 - y1) * (point.getVector3fMap ()[PlaneDim1] - x1))
223 template<
typename Po
intT>
bool
225 const Eigen::Vector3f& ray,
226 const Vertices& verts,
237 assert (verts.vertices.size () == 3);
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);
248 if (std::fabs (n_dot_ray) < 1e-9)
251 const float r = n.dot (a - p) / n_dot_ray;
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;
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)
271 #define PCL_INSTANTIATE_CropHull(T) template class PCL_EXPORTS pcl::CropHull<T>;
Filter points that lie inside or outside a 3D closed surface or 2D closed polygon,...
void applyFilter(PointCloud &output) override
Filter the input points using the 2D or 3D polygon hull.
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.
IndicesAllocator<> Indices
Type used for indices in PCL.
#define PCL_DEPRECATED(Major, Minor, Message)
macro for compatibility across compilers and help remove old deprecated items for the Major....
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.