Robust mean and covariance estimate

[gcasey]

This addresses robustness in covariance estimation that affects normal calculation and RANSAC plane fitting.

[gcasey]

It looks like the failing tests are likely caused by the updated covariance calculation. I'm not sure how the original values were derived so I will update the expected value.

[gcasey]

This change impacts:

• Point normal calculation
• Plane segmentation fitting
• Most point features that rely on normals
• Covariance sampling
• ICP registration with nomral based correspondence rejection

[gcasey]

@VictorLamoine @taketwo @jspricke could someone please take a look at this change? In my experience this significantly improves robustness when point clouds are not near the world origin for normal calculation and RANSAC plane fitting (and anything those depend on).

[jspricke]

Copying @gedikli comment from #1411 over here:

There is a reason why we did the single pass Mean and Covar estimation -> speed.
If the double pass is required, use those methods directly or call the single-pass with Eigen::Matrix3d matrices, which uses double as accumulator. That should also increase the accuracy.

[jspricke]

Maybe we should provide both ways and add a comment/flag in the API?

[gcasey]

I recognize the computation time concerns but based on the current evidence, I still think this is a good change.

I am not sure that it will be easy to provide both methods as a choice in the API. This computation fundamental to several functions (normals, features, plane fitting), and it seems hard to manage that choice in the API to pass through the appropriate filters. The only obvious way to me is to provide a CMake variable or similar. That seems to significantly increase the complexity of using the toolkit to understand the robustness/speed tradeoff.

Before considering giving users an option, I would also like to see evidence that the one-pass version is significantly faster. I'm skeptical that the speed benefit is of enough benefit to outweigh the cost of the robustness.

There have been a variety of bug reports on this topic over the last couple years so there are obviously many users encountering the issue. To me this indicates we should prefer improving the robustness even at the expense of computation time. I am in favor or preferring robust results over fast results that may be incorrect.

I'm skeptical that using double precision in a single pass will be faster or that implementing a robust one-pass algorithm will be better since it brings a division inside the inner loop, but I have not tried either of these. If someone else would like to implement that I am happy to compare the results.

I would be happy to run a before/after performance benchmark on this change if someone can point me to an existing benchmark tool or test.

[junyang224]

Hello gcasey:

I replaced the file "centroid.hpp" with your updated version, but the result unchanged. The following is my code, is it the right way to compute the normal?

pcl::PointCloud::Ptr cloud (new pcl::PointCloud ());

const Eigen::Vector3f translate (15.0, 0.0, 0.0);
const Eigen::Quaternionf no_rotation (0, 0, 0, 0);
pcl::transformPointCloud(*cloud, *cloud, translate, no_rotation);

pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud);

pcl::search::KdTreepcl::PointXYZ::Ptr tree (new pcl::search::KdTreepcl::PointXYZ ());
ne.setSearchMethod (tree);

pcl::PointCloudpcl::Normal::Ptr cloud_normals (new pcl::PointCloudpcl::Normal);
ne.setKSearch(10);
ne.compute (*cloud_normals);

[gcasey]

@CSI4133 I dont see where the actual values for the point cloud are read or set. This fix will affect points clouds that are far from the origin (e.g. at least in the 100-1000 magnitude range )

[junyang224]

@gcasey I read the pcd file from my disk, and tested the result before and after the translation on old and updated version of "centroid.hpp". The result still changed after the translation, but I'm not sure whether I used it in the right way. You only modified the file "common/include/pcl/common/impl/centroid.hpp", right? But even I deleted this file, the code can still be compiled and got the result (exact same as the old result).

The file "common/include/pcl/common/impl/centroid.hpp" seems to be not used, so I am thinking whether I used it correctly.

[junyang224]

@gcasey I just realized that you were using "computePointNormal" to compute each single point normal instead of all of them by "pcl::Feature<PointInT, PointOutT>::compute (PointCloudOut &output)" in "feature.hpp". I followed you and the test result is right (unchanged after translation and exact same curvature value after rotation). I guess it can be a big problem to use feature method to compute normals, and I also assume that even without the updated version of file "centroid.hpp", the difference of normals after translation should not be that bad.

[loganbyers]

For what it's worth, I implemented this new centroid.hpp file with the two-pass covariance computation and found that it was not effective enough for my clouds, as I still had overflow errors (presumably) and bad estimation of normals. I "solved" this by increasing the precision to double for the covariance_matrix_ and xyz_centroid_ member variables of NormalEstimation in normal_3d.h. This relies on casting these two members back to float when passing to solvePlaneParameters to get the actual normals. I haven't done any tests to see what effects changing these two members will have on the rest of the library, but these are private members so they probably aren't being accessed outside of the class.

[SergioRAgostinho]

Aiiiighhhhhht! Time to pick up this conversation again. So I do agree with what @gedikli commented

There is a reason why we did the single pass Mean and Covar estimation -> speed.
If the double pass is required, use those methods directly or call the single-pass with Eigen::Matrix3d matrices, which uses double as accumulator. That should also increase the accuracy.

There are already methods which compute the centroid and the covar separately effectively doing it in a two pass procedure, so computeMeanAndCovarianceMatrix should be left as it is. In light of the comment made by @jspricke , I would change the its name to computeMeanAndCovarianceMatrixSinglePass and would create a wrapper like

template <typename PointT, typename Scalar> inline unsigned int
pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
                                      Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
                                      Eigen::Matrix<Scalar, 4, 1> &centroid, 
                                      bool double_pass = false)
{
    if (double_pass)
    {
      pcl::compute3DCentroid(cloud, centroid);
      pcl::computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix);
    }
    else
      pcl::computeMeanAndCovarianceMatrixSinglePass (cloud, covariance_matrix, centroid);
}

probably even create an inlined wrapper named computeMeanAndCovarianceMatrixDoublePass just for consistency.
Then for all algorithms which are currently being affected by this precision issue, trigger this double_pass flag, which might require exposing it as a flag to their public interfaces.

[jspricke]

+1 for providing both algorithms and having a wrapper.

[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.

[mattforestyoung]

@loganbyers sorry to exhume an old issue, and this might be a long shot... But are you able to share the code you used to achieve this?

I "solved" this by increasing the precision to double for the covariance_matrix_ and xyz_centroid_ member variables of NormalEstimation in normal_3d.h. This relies on casting these two members back to float when passing to solvePlaneParameters to get the actual normals. I haven't done any tests to see what effects changing these two members will have on the rest of the library, but these are private members so they probably aren't being accessed outside of the class.

[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.

[mvieth]

Superseded by pull request #4983