[features] NormalEstimationOMP possible optimization

[roncapat]

Is your feature request related to a problem? Please describe.

Code using NormalEstimationOMP built with PCL 1.14, RelWithDebInfo + -march=native.
Profiled with perf, I see that around 30% of the time spent is calling pcl::isFinite().

Context

Trying to compute cloud normals with NormalEstimationOMP at a predetermined loop rate for traversability estimation purposes (autonomous navigation).

Expected behavior

Some time savings, like the possibility to precompute isFinite() once and looking it up when needed.

Current Behavior

pcl/search/include/pcl/search/impl/organized.hpp

Lines 81 to 86 in 56a3172

	for (; xEnd != yEnd; idx += skip, xEnd += input_->width)
	{
	for (; idx < xEnd; ++idx)
	{
	if (!mask_[idx] || !isFinite ((*input_)[idx]))
	continue;

here isFinite is called for every point in the RadiusSearchBox: this also means that a point may be checked multiple times!

pcl/common/include/pcl/common/impl/centroid.hpp

Lines 615 to 618 in 56a3172

	for (const auto &index : indices)
	{
	if (!isFinite (cloud[index]))
	continue;

here we call again isFinite.

Both the above linked codepaths are called by NormalEstimationOMP.

Describe the solution you'd like

Both the above linked codepaths are called by NormalEstimationOMP. This means that we may choose to pre-compute over the whole cloud an isFinite matrix/mask, propagate it to both code snippets above to look up such values.

[mvieth]

Hi, funny that you bring this up because I have also noticed recently that points are checked multiple times in the organized search. I have created a pull request to change that: #6160

For computeMeanAndCovarianceMatrix I don't see a very nice and elegant solution. Computing a mask somewhere and passing it to the place where it is needed through 2 or 3 function calls/parameters feels not so great to be honest (lots of code changes and the parameters would have to be optional, so the functions have to check first if the mask is actually valid). On possible work-around that I have used before: compute the indices of all valid points in the cloud, mark the cloud as dense (cloud->is_dense = true), and pass cloud + indices to the normal estimation. is_dense tells the algorithms that they don't have to check isFinite. And indeed they don't have to do that because they will only operate on the indices you gave them.

[roncapat]

Thank you for your quick feedback! I assume that this can be closed pending #6160 then 🚀

[mvieth]

#6160 is merged now