Loading...
Searching...
No Matches
crop_hull.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2011, Willow Garage, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*/
#ifndef PCL_FILTERS_IMPL_CROP_HULL_H_
#define PCL_FILTERS_IMPL_CROP_HULL_H_
#include <pcl/filters/crop_hull.h>
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> void
pcl::CropHull<PointT>::applyFilter (Indices &indices)
{
indices.clear();
removed_indices_->clear();
indices.reserve(indices_->size());
removed_indices_->reserve(indices_->size());
if (dim_ == 2)
{
// in this case we are assuming all the points lie in the same plane as the
// 2D convex hull, so the choice of projection just changes the
// conditioning of the problem: choose to squash the XYZ component of the
// hull-points that has least variation - this will also give reasonable
// results if the points don't lie exactly in the same plane
const Eigen::Vector3f range = getHullCloudRange ();
if (range[0] <= range[1] && range[0] <= range[2])
applyFilter2D<1,2> (indices);
else if (range[1] <= range[2] && range[1] <= range[0])
applyFilter2D<2,0> (indices);
else
applyFilter2D<0,1> (indices);
}
else
{
applyFilter3D (indices);
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> Eigen::Vector3f
pcl::CropHull<PointT>::getHullCloudRange ()
{
Eigen::Vector3f cloud_min (
std::numeric_limits<float>::max (),
std::numeric_limits<float>::max (),
std::numeric_limits<float>::max ()
);
Eigen::Vector3f cloud_max (
-std::numeric_limits<float>::max (),
-std::numeric_limits<float>::max (),
-std::numeric_limits<float>::max ()
);
for (pcl::Vertices const & poly : hull_polygons_)
{
for (auto const & idx : poly.vertices)
{
Eigen::Vector3f pt = (*hull_cloud_)[idx].getVector3fMap ();
cloud_min = cloud_min.cwiseMin(pt);
cloud_max = cloud_max.cwiseMax(pt);
}
}
return (cloud_max - cloud_min);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> template<unsigned PlaneDim1, unsigned PlaneDim2> void
pcl::CropHull<PointT>::applyFilter2D (Indices &indices)
{
for (std::size_t index = 0; index < indices_->size (); index++)
{
// iterate over polygons faster than points because we expect this data
// to be, in general, more cache-local - the point cloud might be huge
std::size_t poly;
for (poly = 0; poly < hull_polygons_.size (); poly++)
{
if (isPointIn2DPolyWithVertIndices<PlaneDim1,PlaneDim2> (
(*input_)[(*indices_)[index]], hull_polygons_[poly], *hull_cloud_
))
{
if (crop_outside_)
indices.push_back ((*indices_)[index]);
// once a point has tested +ve for being inside one polygon, we can
// stop checking the others:
break;
}
}
// If we're removing points *inside* the hull, only remove points that
// haven't been found inside any polygons
if (poly == hull_polygons_.size () && !crop_outside_)
indices.push_back ((*indices_)[index]);
if (indices.empty() || indices.back() != (*indices_)[index]) {
removed_indices_->push_back ((*indices_)[index]);
}
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> void
pcl::CropHull<PointT>::applyFilter3D (Indices &indices)
{
// This algorithm could definitely be sped up using kdtree/octree
// information, if that is available!
for (std::size_t index = 0; index < indices_->size (); index++)
{
// test ray-crossings for three random rays, and take vote of crossings
// counts to determine if each point is inside the hull: the vote avoids
// tricky edge and corner cases when rays might fluke through the edge
// between two polygons
// 'random' rays are arbitrary - basically anything that is less likely to
// hit the edge between polygons than coordinate-axis aligned rays would
// be.
std::size_t crossings[3] = {0,0,0};
Eigen::Vector3f rays[3] =
{
Eigen::Vector3f(0.264882f, 0.688399f, 0.675237f),
Eigen::Vector3f(0.0145419f, 0.732901f, 0.68018f),
Eigen::Vector3f(0.856514f, 0.508771f, 0.0868081f)
};
for (const auto & hull_polygon : hull_polygons_)
for (std::size_t ray = 0; ray < 3; ray++)
crossings[ray] += rayTriangleIntersect
((*input_)[(*indices_)[index]], rays[ray], hull_polygon, *hull_cloud_);
bool crosses = (crossings[0]&1) + (crossings[1]&1) + (crossings[2]&1) > 1;
if ((crop_outside_ && crosses) || (!crop_outside_ && !crosses))
indices.push_back ((*indices_)[index]);
else
removed_indices_->push_back ((*indices_)[index]);
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> template<unsigned PlaneDim1, unsigned PlaneDim2> bool
pcl::CropHull<PointT>::isPointIn2DPolyWithVertIndices (
const PointT& point, const Vertices& verts, const PointCloud& cloud)
{
bool in_poly = false;
double x1, x2, y1, y2;
const int nr_poly_points = static_cast<int>(verts.vertices.size ());
double xold = cloud[verts.vertices[nr_poly_points - 1]].getVector3fMap ()[PlaneDim1];
double yold = cloud[verts.vertices[nr_poly_points - 1]].getVector3fMap ()[PlaneDim2];
for (int i = 0; i < nr_poly_points; i++)
{
const double xnew = cloud[verts.vertices[i]].getVector3fMap ()[PlaneDim1];
const double ynew = cloud[verts.vertices[i]].getVector3fMap ()[PlaneDim2];
if (xnew > xold)
{
x1 = xold;
x2 = xnew;
y1 = yold;
y2 = ynew;
}
else
{
x1 = xnew;
x2 = xold;
y1 = ynew;
y2 = yold;
}
if ((xnew < point.getVector3fMap ()[PlaneDim1]) == (point.getVector3fMap ()[PlaneDim1] <= xold) &&
(point.getVector3fMap ()[PlaneDim2] - y1) * (x2 - x1) < (y2 - y1) * (point.getVector3fMap ()[PlaneDim1] - x1))
{
in_poly = !in_poly;
}
xold = xnew;
yold = ynew;
}
return (in_poly);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> bool
pcl::CropHull<PointT>::rayTriangleIntersect (const PointT& point,
const Eigen::Vector3f& ray,
const Vertices& verts,
const PointCloud& cloud)
{
// Algorithm here is adapted from:
// http://softsurfer.com/Archive/algorithm_0105/algorithm_0105.htm#intersect_RayTriangle()
//
// Original copyright notice:
// Copyright 2001, softSurfer (www.softsurfer.com)
// This code may be freely used and modified for any purpose
// providing that this copyright notice is included with it.
//
assert (verts.vertices.size () == 3);
const Eigen::Vector3f p = point.getVector3fMap ();
const Eigen::Vector3f a = cloud[verts.vertices[0]].getVector3fMap ();
const Eigen::Vector3f b = cloud[verts.vertices[1]].getVector3fMap ();
const Eigen::Vector3f c = cloud[verts.vertices[2]].getVector3fMap ();
const Eigen::Vector3f u = b - a;
const Eigen::Vector3f v = c - a;
const Eigen::Vector3f n = u.cross (v);
const float n_dot_ray = n.dot (ray);
if (std::fabs (n_dot_ray) < 1e-9)
return (false);
const float r = n.dot (a - p) / n_dot_ray;
if (r < 0)
return (false);
const Eigen::Vector3f w = p + r * ray - a;
const float denominator = u.dot (v) * u.dot (v) - u.dot (u) * v.dot (v);
const float s_numerator = u.dot (v) * w.dot (v) - v.dot (v) * w.dot (u);
const float s = s_numerator / denominator;
if (s < 0 || s > 1)
return (false);
const float t_numerator = u.dot (v) * w.dot (u) - u.dot (u) * w.dot (v);
const float t = t_numerator / denominator;
if (t < 0 || s+t > 1)
return (false);
return (true);
}
#define PCL_INSTANTIATE_CropHull(T) template class PCL_EXPORTS pcl::CropHull<T>;
#endif // PCL_FILTERS_IMPL_CROP_HULL_H_
pcl::CropHull
Filter points that lie inside or outside a 3D closed surface or 2D closed polygon,...
Definition crop_hull.h:52
pcl::CropHull::applyFilter
void applyFilter(Indices &indices) override
Filter the input points using the 2D or 3D polygon hull.
Definition crop_hull.hpp:46
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition point_cloud.h:173
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition point_types.hpp:623
pcl::Vertices
Describes a set of vertices in a polygon mesh, by basically storing an array of indices.
Definition Vertices.h:15