[filters] Refatoring VoxelGridCovariance to make it multi-thread safe (and more)

[koide3]

This PR updates VoxelGridCovariance so that it can be used in multi-threaded NDT #4135

• getNeighborhoodAtPoint is generalized so that any neighborhood patterns can be designated
• const qualifiers are added to getNeighborhoodAtPoint, nearestKSearch, and radiusSearch to clarify that they are thread-safe

Although the second modification slightly changes the behavior of the methods in case a voxel found by kdtree_ doesn't exist in leaves_, this condition never happens because kdtree_ is built from voxel coordinates in leaves_.

Regarding #4180 (comment):
I empirically found that using adjacent + center voxels (7 voxels) is the best for accuracy. I updated the doc comment of getAdjacentVoxelsAtPoint to clarify that it retrieves 7 voxels.

[kunaltyagi]

Just a few questions:

• Should the added function require Eigen<int, 4, Dynamic> instead since that's the sole use in the function?
• What happens if the neighborhood of a point contains the point itself? Does the accuracy increase in a similar fashion as in case of adjacent voxels?
• Do we need a better naming than nighborhood and adjacentvoxels? I can see someone getting confused between the 2

[koide3]

@kunaltyagi

Should the added function require Eigen<int, 4, Dynamic> instead since that's the sole use in the function?

I prefer Eigen<int, 3, Dynamic because we don't need to worry about what values should be placed at the fourth column. If it's ok, I would like to keep the current form.

What happens if the neighborhood of a point contains the point itself? Does the accuracy increase in a similar fashion as in case of adjacent voxels?

Possibly yes. I tried to use 27 neighbors once, and it was better than 26 neighbors (in both accuracy and processing speed). But, it was much slower than 7 neighbors, and the accuracy gain was not significant. If you like, we can add a function to retrieve 27 neighbors as well.

Do we need a better naming than nighborhood and adjacentvoxels? I can see someone getting confused between the 2

Should we rename them like get7NeighborhoodAtPoint and get26NeighborhoodAtPoint?

[kunaltyagi]

Should we rename them like get7NeighborhoodAtPoint and get26NeighborhoodAtPoint?

getFaceNeighbors and AllNeighbors? The second makes much less sense than the former.

[kunaltyagi]

If you like, we can add a function to retrieve 27 neighbors as well

That'd be nice since it's better than 26 neighbors.

because we don't need to worry about what values should be placed at the fourth column

Makes sense. Please ignore that.

LGTM, can be merged once a good enough name and the next bit are done.

[koide3]

getFaceNeighbors and AllNeighbors? The second makes much less sense than the former.
That'd be nice since it's better than 26 neighbors.

I added a method to get 3x3x3 neighbors and changed the method names (7neighbors: getFaceNeighborsAtPoint, 27: getAllNeighborsAtPoint)

I considered to change the name of 26neighbors method to getSurroundingVoxelsAtPoint, but I noticed that it's public. Is it allowed to change a public method name? (AFAIK, it's used only in NDT though)

[kunaltyagi]

Is it allowed to change a public method name?

Yes, via deprecation, but let's put that for another patch. This one doesn't have any issues from being included in 1.11.1, but addition of deprecation would force it to 1.12.0

[kunaltyagi]

Any changes (rebasing of commits) or we can go ahead and merge (squash and merge)?

[SergioRAgostinho]

I was gonna squash merge. 😬

[kunaltyagi]

You didn't so I thought you were waiting on something. Merging now.