38 #ifndef PCL_FILTERS_IMPL_CROP_HULL_H_
39 #define PCL_FILTERS_IMPL_CROP_HULL_H_
41 #include <pcl/filters/crop_hull.h>
44 template<
typename Po
intT>
void
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);
60 applyFilter2D<0,1> (output);
64 applyFilter3D (output);
69 template<
typename Po
intT>
void
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);
85 applyFilter2D<0,1> (indices);
89 applyFilter3D (indices);
94 template<
typename Po
intT> Eigen::Vector3f
97 Eigen::Vector3f cloud_min (
98 std::numeric_limits<float>::max (),
99 std::numeric_limits<float>::max (),
100 std::numeric_limits<float>::max ()
102 Eigen::Vector3f cloud_max (
103 -std::numeric_limits<float>::max (),
104 -std::numeric_limits<float>::max (),
105 -std::numeric_limits<float>::max ()
107 for (std::size_t index = 0; index < indices_->size (); index++)
109 Eigen::Vector3f pt = (*input_)[(*indices_)[index]].getVector3fMap ();
110 for (
int i = 0; i < 3; i++)
112 if (pt[i] < cloud_min[i]) cloud_min[i] = pt[i];
113 if (pt[i] > cloud_max[i]) cloud_max[i] = pt[i];
117 return (cloud_max - cloud_min);
121 template<
typename Po
intT>
template<
unsigned PlaneDim1,
unsigned PlaneDim2>
void
124 for (std::size_t index = 0; index < indices_->size (); index++)
129 for (poly = 0; poly < hull_polygons_.size (); poly++)
131 if (isPointIn2DPolyWithVertIndices<PlaneDim1,PlaneDim2> (
132 (*input_)[(*indices_)[index]], hull_polygons_[poly], *hull_cloud_
136 output.
push_back ((*input_)[(*indices_)[index]]);
144 if (poly == hull_polygons_.size () && !crop_outside_)
145 output.
push_back ((*input_)[(*indices_)[index]]);
150 template<
typename Po
intT>
template<
unsigned PlaneDim1,
unsigned PlaneDim2>
void
154 for (std::size_t index = 0; index < indices_->size (); index++)
157 for (poly = 0; poly < hull_polygons_.size (); poly++)
159 if (isPointIn2DPolyWithVertIndices<PlaneDim1,PlaneDim2> (
160 (*input_)[(*indices_)[index]], hull_polygons_[poly], *hull_cloud_
164 indices.push_back ((*indices_)[index]);
168 if (poly == hull_polygons_.size () && !crop_outside_)
169 indices.push_back ((*indices_)[index]);
174 template<
typename Po
intT>
void
180 for (std::size_t index = 0; index < indices_->size (); index++)
189 std::size_t crossings[3] = {0,0,0};
190 Eigen::Vector3f rays[3] =
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)
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_);
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]]);
210 template<
typename Po
intT>
void
214 for (std::size_t index = 0; index < indices_->size (); index++)
216 std::size_t crossings[3] = {0,0,0};
217 Eigen::Vector3f rays[3] =
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)
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_);
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]);
237 template<
typename Po
intT>
template<
unsigned PlaneDim1,
unsigned PlaneDim2>
bool
241 bool in_poly =
false;
242 double x1, x2, y1, y2;
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++)
249 const double xnew = cloud[verts.vertices[i]].getVector3fMap ()[PlaneDim1];
250 const double ynew = cloud[verts.vertices[i]].getVector3fMap ()[PlaneDim2];
266 if ((xnew < point.getVector3fMap ()[PlaneDim1]) == (point.getVector3fMap ()[PlaneDim1] <= xold) &&
267 (point.getVector3fMap ()[PlaneDim2] - y1) * (x2 - x1) < (y2 - y1) * (point.getVector3fMap ()[PlaneDim1] - x1))
279 template<
typename Po
intT>
bool
281 const Eigen::Vector3f& ray,
282 const Vertices& verts,
293 assert (verts.vertices.size () == 3);
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);
304 if (std::fabs (n_dot_ray) < 1e-9)
307 const float r = n.dot (a - p) / n_dot_ray;
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;
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)
327 #define PCL_INSTANTIATE_CropHull(T) template class PCL_EXPORTS pcl::CropHull<T>;
329 #endif // PCL_FILTERS_IMPL_CROP_HULL_H_