Fix the removed indices of UniformSampling

[wannesvanloock]

This ensures that all removed indices are set and not only those of the indices that are visited.

[SergioRAgostinho]

Hi. Can you give provide more details about the behavior you're trying to correct with this PR? I.e., what was your use case with this class, which was failing with the current implementation.

[wannesvanloock]

I was using the removed indices to filter an organized point cloud, as this class returns an unorganized cloud. What I noticed was that removed_indices.size() + downsampled_cloud.size() < initial_cloud.size(). Basically, the removed_indices do not not include all indices that are actually removed. This is due to the fact that idx within the loop does not visit each index of the input point cloud. As such, we have to keep track of the indices that are visited to be able to return the removed indices.

[SergioRAgostinho]

I had a look at your suggested changes:

• I cannot identify where in the previous code we're skipping indexes, since we're looping the entire index vector provided and always doing one of three things: removing because it's NaN; assigning to a leaf; removing an old assigned index to a leaf, if this new index is closer. No index should have been skipped in this situation. I need some help from your side to understand where exactly we're skipping indexes.
• With your implementation, populating removed_indexes_ now takes 2 passes instead of just 1.
• You're copying the cp instead of (*indices_)[cp], to removed_indices_ which makes me think your expectation of what the algorithm should return might not exactly be correct. removed_indices.size() + downsampled_cloud.size() == initial_cloud.size() will only be true if indices_ is populated with 0, ..., initial_cloud.size() - 1 (and if all leafs are properly populated). If I provide a pointcloud with 100 points, masked by an indices vector with just the index 0, which gets assigned to the single leaf that will exist, at the end the vector will have the following size

removed_indices_.size () = 0
downsampled_cloud.size () = 1
initial_cloud.size() = 100

What is valid though is removed_indices.size() + downsampled_cloud.size() == indices_.size(). Is this not verified in your case?

[wannesvanloock]

I cannot identify where in the previous code we're skipping indexes, since we're looping the entire index vector

We are indeed looping the entire index vector but we're always only considering idx of the indices_ vector which is set by int idx = (ijk - min_b_).dot (divb_mul_);

With your implementation, populating removed_indexes_ now takes 2 passes instead of just 1.

Correct, unfortunately I don't see a better way to set the remove_indices. It would be easier to have the keep_indices in this case.

You're copying the cp instead of (*indices_)[cp], to removed_indices_

This was indeed incorrect from my side and is fixed in the latest commits.

removed_indices.size() + downsampled_cloud.size() == initial_cloud.size() will only be true if indices_ is populated with 0, ..., initial_cloud.size() - 1

Indeed. To clarify my point, I have added two unit test, both which fail on master.

[SergioRAgostinho]

We are indeed looping the entire index vector but we're always only considering idx of the indices_ vector which is set by int idx = (ijk - min_b_).dot (divb_mul_);

Your explanation didn't really make sense to me, because idx is not required to land on all possible indexes of the voxelized grid we create.

I went digging for the issue myself and found the bug. The issue was that we were not adding the points in the condition that their distance was bigger than the one for the index already stored on the leaf. The snippet below should help to clarify my lousy explanation. Your tests (thank you for writing them btw), are running green.

// First pass: build a set of leaves with the point index closest to the leaf center
  for (size_t cp = 0; cp < indices_->size (); ++cp)
  {
   [...]
// Check to see if this point is closer to the leaf center than the previous one we saved
    float diff_cur   = (input_->points[(*indices_)[cp]].getVector4fMap () - ijk.cast<float> ()).squaredNorm ();
    float diff_prev  = (input_->points[leaf.idx].getVector4fMap ()        - ijk.cast<float> ()).squaredNorm ();

    // If current point is closer, copy its index instead
    if (Filter<PointT>::extract_removed_indices_)
      Filter<PointT>::removed_indices_->push_back (diff_cur < diff_prev ? leaf.idx : (*indices_)[cp]);

    if (diff_cur < diff_prev)
      leaf.idx = (*indices_)[cp];
  }

  // Second pass: go over all leaves and copy data
  output.points.resize (leaves_.size ());
  int cp = 0;
  [...]

This was the test I created to trace the issue, because loading a 100k pointcloud is not really tractable for a human 😅

TEST (Full, Uniform_Sampling)
{
  // Construct point cloud
  PointCloud<PointXYZ>::Ptr simple (new PointCloud<PointXYZ>);
  simple->push_back (PointXYZ (0, 0, 0));
  simple->push_back (PointXYZ (0, 0, 1));
  simple->push_back (PointXYZ (0, 0, .99));
  simple->push_back (PointXYZ (0, 0, .98));
  simple->push_back (PointXYZ (0, 1, 0));
  simple->push_back (PointXYZ (0, 1, 1));
  simple->push_back (PointXYZ (1, 0, 0));
  simple->push_back (PointXYZ (1, 0, 1));
  simple->push_back (PointXYZ (1, 1, 0));
  simple->push_back (PointXYZ (1, 1, 1));
  simple->is_dense = true;

  const bool keep_remove_indices = true;
  UniformSampling<PointXYZ> us (keep_remove_indices);
  us.setInputCloud (simple);
  us.setRadiusSearch (0.5);
  PointCloud<PointXYZ>::Ptr cloud_filtered (new PointCloud<PointXYZ> ());
  us.filter (*cloud_filtered);

  const IndicesConstPtr& remove_indices = us.getRemovedIndices ();

  EXPECT_EQ(remove_indices->size () + cloud_filtered->points.size (),
            simple->points.size ());
}

[SergioRAgostinho]

As per my previous commentary.

[wannesvanloock]

Apologies for my absence. Your proposed fix seems indeed the better way to go. Wrt to your proposed test, the expected remove_indices in the case for search_radius=0.5 seems ill defined? Intuitively, I would expect that either (1, 2), (1, 3) or (2, 3) are valid answers. In case of search_radius = 0.5, I only get 3 though. When I set the radius > 0.5 + 1e-7, I do get the expected results.

[SergioRAgostinho]

Intuitively, I would expect that either (1, 2), (1, 3) or (2, 3) are valid answers.

I don't understand what this refers to.

After reading your comment regarding the lack of intuition of radius search parameter, I went digging again to understand the role it plays and in the source code it is used to define the leaf size. So currently leaf_size = radius. Now, this also feels unintuitive to me, because by radius search distance, one expects for there to exist a central point to which all points with distance inferior to a certain threshold, to be eligible search results. I would argue that leaf_size = 2* radius is more intuitive, yet still imprecise. What we should have is a method for setting the leaf size directly, because radial search plays no role in this algorithm. (Opinions @taketwo )

[wannesvanloock]

I wanted to extend your unit test to not only test the number of removed indices, but the algorithm itself. In your test I would expect that only one of the three points (0, 0, 1), (0, 0, .99), (0, 0, .98) would remain. With search_radius = 0.5 however, only (0, 0, .98) is removed. Anything larger than 0.5, also removes (0, 0, .99). I also agree that radial search plays no role in this algorithm.

[taketwo]

Agree about the irrelevance of search radius. What about deprecating setRadiusSearch() and creating a new method to set the leaf size? (Though... is the term "leaf" clear enough in this context? The docstring of the class talks about voxel grid and boxes, so maybe "voxel size" or "cell size" is more intuitive.) Further, we may get rid of the search_radius_ member field, it's unused.

[SergioRAgostinho]

I wanted to extend your unit test to not only test the number of removed indices, but the algorithm itself. In your test I would expect that only one of the three points (0, 0, 1), (0, 0, .99), (0, 0, .98) would remain. With search_radius = 0.5 however, only (0, 0, .98) is removed. Anything larger than 0.5, also removes (0, 0, .99). I also agree that radial search plays no role in this algorithm.

That is definitely weird... According to the comments in the code, only the point closest to the leaf center should be preserved. I'm assuming the leaf center in this case to be at (0.25 , 0.25, 0.75) which means that only (0, 0, .98) should be preserved. Is the leaf center somewhere else?

Agree about the irrelevance of search radius. What about deprecating setRadiusSearch() and creating a new method to set the leaf size? (Though... is the term "leaf" clear enough in this context? The docstring of the class talks about voxel grid and boxes, so maybe "voxel size" or "cell size" is more intuitive.) Further, we may get rid of the search_radius_ member field, it's unused.

Agreed. I think the term leaf size is also used for instance in the voxel grid filter.

[stale]

This pull request has been automatically marked as stale because it hasn't had
any activity in the past 60 days. Commenting or adding a new commit to the
pull request will revert this.

Come back whenever you have time. We look forward to your contribution.

[stale]

This pull request has been automatically marked as stale because it hasn't had
any activity in the past 60 days. Commenting or adding a new commit to the
pull request will revert this.

Come back whenever you have time. We look forward to your contribution.

[stale]

This pull request has been automatically marked as stale because it hasn't had
any activity in the past 60 days. Commenting or adding a new commit to the
pull request will revert this.

Come back whenever you have time. We look forward to your contribution.