add robust covariance matrix calculation to normal_3d.h #3169

dominiknatter · 2019-06-18T06:27:03Z

If points used for a covariance matrix calculation are far away from the origin, numerical issues can lead to unreliable results, as discussed in #2403 . For normal estimation a fix was implemented, that subtracts the center point of the neighborhood prior to the covariance calculation. Thus all values are close to zero and no numerical issues occur. This fix is based on the original function in centroid.h but was tailored to normal estimation, i.e. finite neighborhoods are expected. Thus it's implemented in normal_3d.h.

Various experiments were conducted to check the functionality. Even if point clouds are shifted far away from the origin, this fix results in reliable normals whereas the current implementation leads to messy normals. Also in terms of run time the code can compete and is even faster for non-dense clouds, as the check for this property is omitted (not needed because finite neighborhoods are a prerequisite in normal estimation).

If points used for a covariance matrix calculation are far away from the origin, numerical issues can lead to unreliable results. For normal estimation a fix was implemented, that subtracts the center point of the neighborhood. Thus all values are close to zero and no numerical issues occur.

taketwo · 2019-06-23T20:39:35Z

Originally we considered integrating this fix into the "main" implementation of computeMeanAndCovarianceMatrix(). However, as this function is quite generic and may be used in different contexts (not only normal estimation), some questions popped up. Should the fix just replace the original implementation? Or should both be available? If so, which one is the default? How to design user interface for switching between these options? Compile or run-time? We did not have good answers to any of these, so we decided to implant the fix exactly where we need it, i.e. in the normal estimation header.

Speaking of runtime, here are the results of some of the experiments that Dominik did:

@SergioRAgostinho @msy22 Any thoughts on this?

@dominiknatter right now you simply added a new function. We will need to update the normal estimation code to actually use it. Also a short unit test and a few more lines in the documentation will be needed, but more on that later.

msy22 · 2019-06-23T22:21:59Z

The run time comparison looks good! I am curious as to why the proposed code is actually faster than the existing code for the Kinect and Clock Tower data sets.

As for how to implement the fix... my thoughts are:

My first thought is that if the proposed solution is about as fast if not faster than the current method, then it seems like a direct improvement. And you could argue that the potential performance boost alone justifies making the proposed code the new default.
If there is a case for having two implementations of computeMeanAndCovariance my gut feeling is to make the most robust or "safest" option the default. Especially given how loss of significance isn't a well-publicized problem and most people want to use PCL as a "black box", without having to dig into the source code when or if something goes wrong.
Further, if we assumed that the existing code is faster across a greater range of clouds (although @dominiknatter's results suggest otherwise), in my experience when libraries have less stable but faster options, they are usually not the default. So a user who wants to get more performance out of their code has to go looking for them. This often means the user is more likely to know what they are doing, or at least more likely to assume that there is some risk/downside given that it is not the default option.
I think it is fair to call the current method "unstable" or "imprecise" with large coordinates, so if we did make it available as a separate option, I would be inclined to rename it as a separate function with a name like computeMeanAndCovarianceUnstable, as it explicitly communicates a level of risk to the user.

In summary: Make the proposed code the default for computeMeanAndCovariance. If there is a case for the current code to continue to exist, give it a separate definition and name that explicitly communicates the risk of using it.

@taketwo and @SergioRAgostinho, you both have far greater experience with PCL and large code bases than me, so feel free to correct me if any of my reasoning or assumptions are wrong.

SergioRAgostinho · 2019-06-23T22:27:43Z

Thank you for the introduction. I saw this PR but was unsure if this was from your colleague since it was only targeting normal estimation.

We did not have good answers to any of these, so we decided to implant the fix exactly where we need it, i.e. in the normal estimation header.

Currently you are creating a specialized replica of an existing functionality and it would obviously be great if we could manage to do everything in a unified way, for maintainability's sake. I need to have a better look to see if I can suggest something. However, given your previous brainstorm, I'm not very confident.

The benchmarks look pretty. It would be good to have @msy22 actually run his error benchmarks to confirm this actually gets rid of the original problem.

@msy22 : I agree with all your assessments.

As is, I'm a little sad to leave this new method just available for normal estimation. ~~I feel it should be an essential part of the common module.~~ Updated based on:

However, as I mentioned before, the boost in performance for Kinect clouds is actually unrelated to the fix and comes from adding an assumption that is valid for our normal estimation approach, but may not be true in general case.

SergioRAgostinho

Brief review. You might want to run your timing benchmarks on some of the changes I proposed. They are based on some expectations I have, but they might actually hinder performance without me being aware.

SergioRAgostinho · 2019-06-23T22:32:27Z

features/include/pcl/features/normal_3d.h

+      * \return size of the input cloud.
+      * \ingroup common
+      */
+      inline unsigned int


This is a big function. This inline is uncalled for.

SergioRAgostinho · 2019-06-23T22:39:00Z

features/include/pcl/features/normal_3d.h

+                                              Eigen::Matrix<float, 4, 1> &centroid)
+      {
+      // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
+      Eigen::Matrix<float, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<float, 1, 9, Eigen::RowMajor>::Zero ();


There's no need to explicitly specify row/col major in a vector. Also, you're accessing all element individually in the code bellow. A std::array would be just fine.

This (and the previous inline) all comes directly from the original implementation. If we want to change, then I'd change both.

Let's update both then.

We need a decision first whether we'll keep both :)

SergioRAgostinho · 2019-06-23T22:40:58Z

features/include/pcl/features/normal_3d.h

+      // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
+      Eigen::Matrix<float, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<float, 1, 9, Eigen::RowMajor>::Zero ();
+      size_t point_count;
+      PointInT center = cloud[indices[0]];


Take a const reference the point.

The case if indices is an empty vector, e.g. no neighbors, must be considered.

Add that extra logic then. My original inline comment is still applicable.

SergioRAgostinho · 2019-06-23T22:42:56Z

features/include/pcl/features/normal_3d.h

+      Eigen::Matrix<float, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<float, 1, 9, Eigen::RowMajor>::Zero ();
+      size_t point_count;
+      PointInT center = cloud[indices[0]];
+      PointInT p;


Under normal circumstances p should be declared inside the for-loop.

SergioRAgostinho · 2019-06-23T22:52:20Z

features/include/pcl/features/normal_3d.h

+        //const PointT& point = cloud[*iIt];
+        p.x = cloud[index].x - center.x;
+        p.y = cloud[index].y - center.y;
+        p.z = cloud[index].z - center.z;


Any chance this can be vectorized with?

pcl/common/include/pcl/impl/point_types.hpp

Line 192 in 861839f

inline pcl::Vector4fMap getVector4fMap () { return (pcl::Vector4fMap (data)); } \

p.getVector4fMap () = center.getVector4fMap ();

We experimented with this and also 3fMap version and there was no difference in performance.

taketwo · 2019-06-24T07:54:22Z

I am curious as to why the proposed code is actually faster than the existing code for the Kinect and Clock Tower data sets.

It's faster on Kinect because we eliminated the isfinite check. The neighborhood is known to contain only finite points (because it's a result of NN lookup). For other clouds there is no difference because they are "dense" anyway and this check is not performed. As for the ClockTower data, we have no explanation. Something weird is going on.

It would be good to have @msy22 actually run his error benchmarks to confirm this actually gets rid of the original problem.

We ran tests comparing the output of this fix and "ideal fix" (where we subtract actual centroid), they are the same down to fractions of a degree.

@msy22 : I agree with all your assessments.

I also tend to agree. However, as I mentioned before, the boost in performance for Kinect clouds is actually unrelated to the fix and comes from adding an assumption that is valid for our normal estimation approach, but may not be true in general case.

stale · 2020-02-21T19:43:32Z

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.

blackccpie · 2020-03-27T14:02:58Z

features/include/pcl/features/normal_3d.h

+        accu [1] += p.x * p.y;
+        accu [2] += p.x * p.z;
+        accu [3] += p.y * p.y;
+        accu [4] += p.z * p.z;


Hi, I think there is a bug in this line, shouldn't it rather be:
accu [4] += p.y * p.z; ?

blackccpie · 2020-03-27T14:04:53Z

Hi, the Generalized ICP also has embedded covariances computation, could it also benefit from this implementation?

Regards

stale · 2020-05-19T19:59:03Z

Marking this as stale due to 30 days of inactivity. Commenting or adding a new commit to the pull request will revert this.

dominiknatter · 2020-05-20T12:13:51Z

The recent stale bot messages brought this up to my attention again, and being the author of this pull request I feel some responsbility. I am unexperienced with PCL, so I gotta ask: did I miss out on some task to be done from my end that caused this topic to turn stale in the first place?

As far as I understood it, first a distinct decision is needed whether to tailor this to normal estimation only or to replace the default computeMeanAndCovarianceMatrix(), right? Once that is done, this comment suggests that more documentation and tests are needed, and also the code comments of @SergioRAgostinho, @mathue, and @blackccpie need to be considered I guess.

kunaltyagi · 2020-05-21T20:38:36Z

did I miss out on some task to be done from my end that caused this topic to turn stale in the first place

Both parties are at fault. While PCL failed to reach a decision on the placement of the improvement, both you failed to keep the PR updated with your inputs and code updates. Neglect turned the PR stale.

However, if you're interested in carrying this forwards, I'd recommend taking a look at #1407.

That way both the following concerns would be assuaged.

Naming: Robust mean and covariance estimate #1407 address that by making the user opt-in explicitly (along with recommendations in this thread)
Placement: Robust mean and covariance estimate #1407 adds the patch to the central functions. Integrating them with the changes here would "solve" the issue (because there were no doubts regarding the correct place there).

Though, I'd recommend to start a new PR and close this one if you do decide to integrate. Else this PR is alive again, and you can continue with your contribution here.

ghost · 2021-09-17T21:20:17Z

@kunaltyagi are there any current PRs that you recommend looking at to get these changes merged, or still #1407? I'm not super well versed with the internals of PCL, but I'd like to start getting involved with the project!

mvieth · 2021-11-20T14:00:17Z

Superseded by pull request #4983

SergioRAgostinho added module: features needs: code review Specify why not closed/merged yet labels Jun 18, 2019

SergioRAgostinho reviewed Jun 23, 2019

View reviewed changes

SergioRAgostinho mentioned this pull request Jul 15, 2019

NormalEstimationOMP generates invalid normals #3226

Closed

This was referenced Aug 7, 2019

PCL point normals not always calculated correctly in sparse clouds #2403

Open

Normals at edges all go trough origin #3269

Open

Kaju-Bubanja mentioned this pull request Aug 27, 2019

Normal Estimation Using Integral Images not always working #3307

Open

stale bot added the status: stale label Feb 21, 2020

blackccpie reviewed Mar 27, 2020

View reviewed changes

stale bot removed the status: stale label Mar 27, 2020

kunaltyagi added needs: more work Specify why not closed/merged yet and removed needs: code review Specify why not closed/merged yet labels Apr 19, 2020

stale bot added the status: stale label May 19, 2020

stale bot removed the status: stale label May 20, 2020

kunaltyagi added the needs: feedback Specify why not closed/merged yet label May 20, 2020

kunaltyagi mentioned this pull request May 21, 2020

SACSegmentation fails when the pointcloud is too far from origin #3041

Closed

SergioRAgostinho mentioned this pull request Jul 13, 2020

[Normal Estimation] Bug in implementation, compared to Open3D #4270

Closed

kunaltyagi linked an issue Jul 14, 2020 that may be closed by this pull request

PCL point normals not always calculated correctly in sparse clouds #2403

Open

miixam mentioned this pull request Feb 25, 2021

[feature] Bug in the normal calculation of computePointNormal #4625

Closed

ghost mentioned this pull request Sep 30, 2021

pcl::NormalRobustEstimation - computing robust normals at the origin #4965

Closed

mvieth closed this Nov 20, 2021

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

add robust covariance matrix calculation to normal_3d.h #3169

add robust covariance matrix calculation to normal_3d.h #3169

dominiknatter commented Jun 18, 2019

taketwo commented Jun 23, 2019

msy22 commented Jun 23, 2019

SergioRAgostinho commented Jun 23, 2019 •

edited

Loading

SergioRAgostinho left a comment

SergioRAgostinho Jun 23, 2019

SergioRAgostinho Jun 23, 2019

taketwo Jun 24, 2019

SergioRAgostinho Jun 24, 2019

taketwo Jun 24, 2019

SergioRAgostinho Jun 23, 2019

mathue Jul 10, 2019

SergioRAgostinho Jul 13, 2019

SergioRAgostinho Jun 23, 2019

SergioRAgostinho Jun 23, 2019

taketwo Jun 24, 2019

taketwo commented Jun 24, 2019

stale bot commented Feb 21, 2020

blackccpie Mar 27, 2020

blackccpie commented Mar 27, 2020

stale bot commented May 19, 2020

dominiknatter commented May 20, 2020

kunaltyagi commented May 21, 2020

ghost commented Sep 17, 2021

mvieth commented Nov 20, 2021

add robust covariance matrix calculation to normal_3d.h #3169

add robust covariance matrix calculation to normal_3d.h #3169

Conversation

dominiknatter commented Jun 18, 2019

taketwo commented Jun 23, 2019

msy22 commented Jun 23, 2019

SergioRAgostinho commented Jun 23, 2019 • edited Loading

SergioRAgostinho left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

taketwo commented Jun 24, 2019

stale bot commented Feb 21, 2020

Choose a reason for hiding this comment

blackccpie commented Mar 27, 2020

stale bot commented May 19, 2020

dominiknatter commented May 20, 2020

kunaltyagi commented May 21, 2020

ghost commented Sep 17, 2021

mvieth commented Nov 20, 2021

SergioRAgostinho commented Jun 23, 2019 •

edited

Loading