Skip to content

Commit

Permalink
Preset to keep all points in radius outlier removal.
Browse files Browse the repository at this point in the history 
Set those to remove to 0 in the to_keep array.
Fix indexing and minor things.
  • Loading branch information
@larshg
larshg committed Nov 30, 2024
1 parent fc3af6a commit 28efda6
Show file tree
Hide file tree
Showing 3 changed files with 52 additions and 39 deletions.
74 changes: 37 additions & 37 deletions filters/include/pcl/filters/impl/radius_outlier_removal.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,7 +79,9 @@ pcl::RadiusOutlierRemoval<PointT>::applyFilterIndices (Indices &indices)
// The arrays to be used
Indices nn_indices (mean_k);
std::vector<float> nn_dists(mean_k);
std::vector<std::uint8_t> to_keep(indices_->size());
// Set to keep all points and in the filtering set those we don't want to keep, assuming
// we want to keep the majority of the points.
std::vector<std::uint8_t> to_keep(indices_->size(), 1);
indices.resize (indices_->size ());
removed_indices_->resize (indices_->size ());
int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator
Expand All @@ -91,11 +93,11 @@ pcl::RadiusOutlierRemoval<PointT>::applyFilterIndices (Indices &indices)
default(none) \
schedule(dynamic,64) \
firstprivate(nn_indices, nn_dists) \
shared(nn_dists_max, mean_k, indices, to_keep) \
shared(nn_dists_max, mean_k, to_keep) \
num_threads(num_threads_)
for (ptrdiff_t i = 0; i < static_cast<ptrdiff_t>(indices_->size()); i++)
{
const index_t& index = (*indices_)[i];
const auto& index = (*indices_)[i];
// Perform the nearest-k search
const int k = searcher_->nearestKSearch (index, mean_k, nn_indices, nn_dists);

Expand All @@ -104,38 +106,22 @@ pcl::RadiusOutlierRemoval<PointT>::applyFilterIndices (Indices &indices)
bool chk_neighbors = true;
if (k == mean_k)
{
if (negative_)
{
chk_neighbors = false;
if (nn_dists_max < nn_dists[k-1])
{
chk_neighbors = true;
}
}
else
{
chk_neighbors = true;
if (nn_dists_max < nn_dists[k-1])
{
chk_neighbors = false;
}
}
}
else
{
chk_neighbors = negative_;
chk_neighbors = false;
}

// Points having too few neighbors are outliers and are passed to removed indices
// Unless negative was set, then it's the opposite condition
// Points having too few neighbors are outliers
if (!chk_neighbors)
{
to_keep[index] = 0;
to_keep[i] = 0;
continue;
}

// Otherwise it was a normal point for output (inlier)
to_keep[index] = 1;
}
}
// NaN or Inf values could exist => use radius search
Expand All @@ -145,40 +131,54 @@ pcl::RadiusOutlierRemoval<PointT>::applyFilterIndices (Indices &indices)
default(none) \
schedule(dynamic, 64) \
firstprivate(nn_indices, nn_dists) \
shared(nn_dists_max, mean_k, indices, to_keep) \
shared(to_keep) \
num_threads(num_threads_)
for (ptrdiff_t i = 0; i < static_cast<ptrdiff_t>(indices_->size()); i++)
{
const index_t& index = (*indices_)[i];
const auto& index = (*indices_)[i];
if (!pcl::isXYFinite((*input_)[index]))
{
to_keep[i] = 0;
continue;
}

// Perform the radius search
// Note: k includes the query point, so is always at least 1
// last parameter (max_nn) is the maximum number of neighbors returned. If enough neighbors are found so that point can not be an outlier, we stop searching.
const int k = searcher_->radiusSearch (index, search_radius_, nn_indices, nn_dists, min_pts_radius_ + 1);

// Points having too few neighbors are outliers and are passed to removed indices
// Unless negative was set, then it's the opposite condition
if ((!negative_ && k <= min_pts_radius_) || (negative_ && k > min_pts_radius_))
// Points having too few neighbors are outliers
if (k <= min_pts_radius_)
{
to_keep[index] = 0;
to_keep[i] = 0;
continue;
}

// Otherwise it was a normal point for output (inlier)
to_keep[index] = 1;
}
}

for (index_t i=0; i < static_cast<index_t>(to_keep.size()); i++)
if (!negative_)
{
if (to_keep[i] == 1)
for (index_t i=0; i < static_cast<index_t>(to_keep.size()); i++)
{
indices[oii++] = i;
if (to_keep[i] == 1)
{
indices[oii++] = (*indices_)[i];
}
else
{
(*removed_indices_)[rii++] = (*indices_)[i];
}
}
else
{
(*removed_indices_)[rii++] = i;
}
else
{
for (index_t i = 0; i < static_cast<index_t>(to_keep.size()); i++) {
if (to_keep[i] == 0) {
indices[oii++] = (*indices_)[i];
}
else {
(*removed_indices_)[rii++] = (*indices_)[i];
}
}
}

Expand Down
4 changes: 2 additions & 2 deletions filters/include/pcl/filters/radius_outlier_removal.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -151,10 +151,10 @@ namespace pcl
setNumberOfThreads(unsigned int nr_threads = 0)
{
#ifdef _OPENMP
num_threads_ = nr_threads ? nr_threads : omp_get_num_procs();
num_threads_ = nr_threads != 0 ? nr_threads : omp_get_num_procs();
#else
if (num_threads_ != 1) {
PCL_WARN("OpenMP is not available. Keeping number of threads unchanged at 1");
PCL_WARN("OpenMP is not available. Keeping number of threads unchanged at 1\n");
}
num_threads_ = 1;
#endif
Expand Down
13 changes: 13 additions & 0 deletions test/filters/test_filters.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1494,6 +1494,7 @@ TEST (RadiusOutlierRemoval, Filters)
{
// Test the PointCloud<PointT> method
PointCloud<PointXYZ> cloud_out;
PointCloud<PointXYZ> cloud_out_neg;
// Remove outliers using a spherical density criterion
RadiusOutlierRemoval<PointXYZ> outrem;
outrem.setInputCloud (cloud);
Expand All @@ -1506,7 +1507,14 @@ TEST (RadiusOutlierRemoval, Filters)
EXPECT_EQ (cloud_out.width, 307);
EXPECT_TRUE (cloud_out.is_dense);

outrem.setNegative(true);
outrem.filter(cloud_out_neg);

EXPECT_EQ(cloud_out_neg.size(), 90);
EXPECT_TRUE(cloud_out_neg.is_dense);

PointCloud<PointXYZRGB> cloud_out_rgb;
PointCloud<PointXYZRGB> cloud_out_rgb_neg;
// Remove outliers using a spherical density criterion on non-dense pointcloud
RadiusOutlierRemoval<PointXYZRGB> outremNonDense;
outremNonDense.setInputCloud(cloud_organized);
Expand All @@ -1519,6 +1527,11 @@ TEST (RadiusOutlierRemoval, Filters)
EXPECT_EQ(cloud_out_rgb.width, 240801);
EXPECT_TRUE(cloud_out_rgb.is_dense);

outremNonDense.setNegative(true);
outremNonDense.filter(cloud_out_rgb_neg);
EXPECT_EQ(cloud_out_rgb_neg.size(), 66399);
//EXPECT_FALSE(cloud_out_rgb_neg.is_dense);

// Test the pcl::PCLPointCloud2 method
PCLPointCloud2 cloud_out2;
RadiusOutlierRemoval<PCLPointCloud2> outrem2;
Expand Down

0 comments on commit 28efda6

Please sign in to comment.