Skip to content

Commit

Permalink
Fix keep_organized behavior in CropHull filter (PointCloudLibrary#4855)
Browse files Browse the repository at this point in the history 
Co-authored-by: Kunal Tyagi <[email protected]>
  • Loading branch information
@DaniilSNikulin @kunaltyagi
DaniilSNikulin and kunaltyagi authored Aug 3, 2021
1 parent 689bea9 commit 7daf23d
Show file tree
Hide file tree
Showing 3 changed files with 43 additions and 95 deletions.
23 changes: 2 additions & 21 deletions filters/include/pcl/filters/crop_hull.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,8 @@ namespace pcl
using Filter<PointT>::filter_name_;
using Filter<PointT>::indices_;
using Filter<PointT>::input_;

using Filter<PointT>::removed_indices_;

using PointCloud = typename Filter<PointT>::PointCloud;
using PointCloudPtr = typename PointCloud::Ptr;
using PointCloudConstPtr = typename PointCloud::ConstPtr;
Expand Down Expand Up @@ -148,15 +149,6 @@ namespace pcl
*/
Eigen::Vector3f
getHullCloudRange ();

/** \brief Apply the two-dimensional hull filter.
* All points are assumed to lie in the same plane as the 2D hull, an
* axis-aligned 2D coordinate system using the two dimensions specified
* (PlaneDim1, PlaneDim2) is used for calculations.
* \param[out] output The set of points that pass the 2D polygon filter.
*/
template<unsigned PlaneDim1, unsigned PlaneDim2> void
applyFilter2D (PointCloud &output);

/** \brief Apply the two-dimensional hull filter.
* All points are assumed to lie in the same plane as the 2D hull, an
Expand All @@ -168,17 +160,6 @@ namespace pcl
template<unsigned PlaneDim1, unsigned PlaneDim2> void
applyFilter2D (Indices &indices);

/** \brief Apply the three-dimensional hull filter.
* Polygon-ray crossings are used for three rays cast from each point
* being tested, and a majority vote of the resulting
* polygon-crossings is used to decide whether the point lies inside
* or outside the hull.
* \param[out] output The set of points that pass the 3D polygon hull
* filter.
*/
void
applyFilter3D (PointCloud &output);

/** \brief Apply the three-dimensional hull filter.
* Polygon-ray crossings are used for three rays cast from each point
* being tested, and a majority vote of the resulting
Expand Down
90 changes: 16 additions & 74 deletions filters/include/pcl/filters/impl/crop_hull.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,37 +40,25 @@

#include <pcl/filters/crop_hull.h>


//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> void
template<typename PointT>
PCL_DEPRECATED(1, 13, "This is a trivial call to base class method")
void
pcl::CropHull<PointT>::applyFilter (PointCloud &output)
{
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> (output);
else if (range[1] <= range[2] && range[1] <= range[0])
applyFilter2D<2,0> (output);
else
applyFilter2D<0,1> (output);
}
else
{
applyFilter3D (output);
}
FilterIndices<PointT>::applyFilter(output);
}


//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
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
Expand Down Expand Up @@ -121,7 +109,7 @@ pcl::CropHull<PointT>::getHullCloudRange ()

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> template<unsigned PlaneDim1, unsigned PlaneDim2> void
pcl::CropHull<PointT>::applyFilter2D (PointCloud &output)
pcl::CropHull<PointT>::applyFilter2D (Indices &indices)
{
for (std::size_t index = 0; index < indices_->size (); index++)
{
Expand All @@ -135,46 +123,25 @@ pcl::CropHull<PointT>::applyFilter2D (PointCloud &output)
))
{
if (crop_outside_)
output.push_back ((*input_)[(*indices_)[index]]);
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_)
output.push_back ((*input_)[(*indices_)[index]]);
}
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> template<unsigned PlaneDim1, unsigned PlaneDim2> void
pcl::CropHull<PointT>::applyFilter2D (Indices &indices)
{
// see comments in (PointCloud& output) overload
for (std::size_t index = 0; index < indices_->size (); index++)
{
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]);
break;
}
}
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 (PointCloud &output)
pcl::CropHull<PointT>::applyFilter3D (Indices &indices)
{
// This algorithm could definitely be sped up using kdtree/octree
// information, if that is available!
Expand All @@ -188,33 +155,6 @@ pcl::CropHull<PointT>::applyFilter3D (PointCloud &output)
// '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 (std::size_t poly = 0; poly < hull_polygons_.size (); poly++)
for (std::size_t ray = 0; ray < 3; ray++)
crossings[ray] += rayTriangleIntersect
((*input_)[(*indices_)[index]], rays[ray], hull_polygons_[poly], *hull_cloud_);

if (crop_outside_ && (crossings[0]&1) + (crossings[1]&1) + (crossings[2]&1) > 1)
output.push_back ((*input_)[(*indices_)[index]]);
else if (!crop_outside_)
output.push_back ((*input_)[(*indices_)[index]]);
}
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> void
pcl::CropHull<PointT>::applyFilter3D (Indices &indices)
{
// see comments in applyFilter3D (PointCloud& output)
for (std::size_t index = 0; index < indices_->size (); index++)
{
std::size_t crossings[3] = {0,0,0};
Eigen::Vector3f rays[3] =
{
Expand All @@ -232,6 +172,8 @@ pcl::CropHull<PointT>::applyFilter3D (Indices &indices)
indices.push_back ((*indices_)[index]);
else if (!crop_outside_)
indices.push_back ((*indices_)[index]);
else
removed_indices_->push_back ((*indices_)[index]);
}
}

Expand Down
25 changes: 25 additions & 0 deletions test/filters/test_crop_hull.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -356,6 +356,31 @@ TYPED_TEST (PCLCropHullTestFixture, test_cloud_filtering)
}


//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TYPED_TEST (PCLCropHullTestFixture, test_keep_organized)
{
for (auto & entry : this->data_)
{
auto & crop_hull_filter = entry.first;
crop_hull_filter.setKeepOrganized(true);
crop_hull_filter.setUserFilterValue(-10.);
const pcl::PointXYZ defaultPoint(-10., -10., -10.);
for (TestData const & test_data : entry.second)
{
crop_hull_filter.setInputCloud(test_data.input_cloud_);
pcl::PointCloud<pcl::PointXYZ> filteredCloud;
crop_hull_filter.filter(filteredCloud);
ASSERT_EQ (test_data.input_cloud_->size(), filteredCloud.size());
for (size_t i = 0; i < test_data.input_cloud_->size(); ++i)
{
pcl::PointXYZ expectedPoint = test_data.inside_mask_[i] ? test_data.input_cloud_->at(i) : defaultPoint;
ASSERT_XYZ_NEAR(expectedPoint, filteredCloud[i], 1e-5);
}
}
}
}


//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// this test will pass only for 2d case //
template <class T>
Expand Down

0 comments on commit 7daf23d

Please sign in to comment.