PCL point normals not always calculated correctly in sparse clouds

[mattforestyoung]

Your Environment

• Operating System and version: Ubuntu 14.04
• Compiler: GCC v4.8.4
• PCL Version: 1.8.1
• C++ Version: C++11

Context

I'm currently trying to find a point cloud registration method that can accurately match relatively sparse 3D point clouds from one Velodyne HDL-32E. At the moment I am using PCL's implementation of Generalized ICP to match several sequential scans similar to the one shown below.

(This is one scan of a carpark outside an office building with floor-ceiling glass windows, hence little few LiDAR returns from the wall).

I.e. I take one scan like this and try to register it to a scan taken 1-2 cm further forward. Now, matching sequential scans does work in some areas but not in others. Since GICP depends on good normal calculations I've been inspecting some clouds in the PCL visualizer (shown in this tutorial) and I noticed that in some places the normals are not being calculated correctly at all.

Expected Behavior

The normals in most parts of the clouds should be calculated correctly. Or if they aren't, it's obviously because the data is too sparse in that region of the point cloud.

Current Behavior

The normal directions are mostly correct, but in some places where there is plenty of data, they are not calculated correctly. The most obvious place is in this example, where the normals all point more-or-less upwards in the flower bed part of the wall. This is incorrect, the normals should be orthogonal to the wall and pointing out horozontally:

This is the case in this specific area pretty much regardless of whether I use a KNearest or Radius point search, and regardless of the values for K or for the radius.

Code to Reproduce

Try the cloud in question: carpark_cloud.ply

in the PCL visualizer (shown in this tutorial)

Possible Solution

I have two theories on what the cause of the problem is.

1. The data itself is the problem. Because the Velodyne has an error of +/- 2cm any given scan line is not a perfect line but has a thickness, whose maximum width will be about 4 cm (assuming max +/- 2cm error). This means that each scan line actually generates it's own plane of points, and at close ranges there may be enough points packed tightly together in one line that this overrules the plane fitting method PCL uses by default and fits a plane to the scan line rather than the underlying topography. Hopefully this diagram accurately illustrates the problem:

2. There is a problem or bug with PCL's plane fitting code. This was suggested to me by a colleague who suggested that because they're all pointing upwards, it might be because that is the default and if the plane fitting functions fail, it won't produce anything different. Now, I've tracked the normal calculation code down to pcl::eigen33, where the smallest eigenvalue is found, and it's associated eigenvector is set to the nx, ny and nz normal coordiantes. But I haven't gotten any further than that. yet. This theory is the reason I'm raising the issue here and not as a general issue on the mailing list.

So my question is: Which of these two theories is likely to be correct, and are there any suggestions on what I can do to resolve it?

[SergioRAgostinho]

Hey. Here's a couple of comments/ideas.

This is incorrect, the normals should be orthogonal to the wall and pointing out horozontally

The points inside the highlighted region appear to be distributed as lines. There might be a chance that during normal estimation, your neighborhood search parameters only allow it to establish a neighborhood which is approximately a line, yielding unreliable normals. Can you randomly select a point used in the region you selected and highlight the points that are being used to estimate the normals?

2. There is a problem or bug with PCL's plane fitting code. This was suggested to me by a colleague who suggested that because they're all pointing upwards, it might be because that is the default and if the plane fitting functions fail, it won't produce anything different. Now, I've tracked the normal calculation code down to pcl::eigen33, where the smallest eigenvalue is found, and it's associated eigenvector is set to the nx, ny and nz normal coordiantes. But I haven't gotten any further than that. yet. This theory is the reason I'm raising the issue here and not as a general issue on the mailing list.

There's a long lasting known problem caused by precision issues. You can read more about it in #560 and #1407. I also believed we also pushed a couple of PRs to master which affect covariance and eigenvalue/vector computation. You should definitely try the master branch if you can, just to see if you verify a different behavior.

Give it a try and report results.

[mattforestyoung]

Thanks very much for the help and the suggestions Serigo! I really appreciate it.

First off, the point selection - this has been something I've worried about. So I did spend some time modifying some of my code to return the selected indices and colour them, which for this specific cloud yields:

And the result is more or less the same, regardless of the input parameters. And in the picture with the normals shown (the one where the cloud is green instead of white) I used a search radius set to 0.3 m, which as you can see from the image just above, there should be more than enough points across multiple lines to produce an accurate normal.

[mattforestyoung]

As for the second suggestion...

Issue #560 talks about the issue and at the bottom, gcasey says that PR #1407 should fix it. But it looks like that PR hasn't been merged with the main branch? And appears to be failing a build check?

It also looks like both you and Logan Byers proposed solutions to the problem in the comments of PR #1407, and that in Logan's case he found the two-pass solution wasn't robust/accurate enough for him. So I'll try his solution and see if I can get it to work...

For the record I did (re-) install PCL 1.8.1 from source (the main trunk) less than two months ago, so it is relatively new.

[SergioRAgostinho]

I used a search radius set to 0.3 m, which as you can see from the image just above, there should be more than enough points across multiple lines to produce an accurate normal.

That's our baseline then and I agree that that should work to produce a decent normal.

Issue #560 talks about the issue and at the bottom, gcasey says that PR #1407 should fix it. But it looks like that PR hasn't been merged with the main branch? And appears to be failing a build check?

It also looks like both you and Logan Byers proposed solutions to the problem in the comments of PR #1407, and that in Logan's case he found the two-pass solution wasn't robust/accurate enough for him. So I'll try his solution and see if I can get it to work...

You should try to compute things manually now for a single point. The normal is the eigen vector associated with the lowest eigenvalue of the covariance matrix. The most robust version of this algorithm I can think is resorting to the following steps, performing them separately with double precision:

• compute the centroid
• demean the points
• compute covariance
• compute eigenvalues/eigenvectors
• retain the eigenvector corresponding to the lowest eigenvalue

Try to reproduce these steps with the highest precision available and validate the results with other libraries whenever possible e.g., once you have the covariance matrix (3x3) you can pass see what numpy outputs as eigenvalues/eigenvectors vs what the library is doing. Try to validate as much steps as possible.

If you reach a stable setup for the normals then report back and we'll start digging when are things failing in the normal estimation algorithm.

[mattforestyoung]

OK, so I kinda raced ahead and went for a full blown solution. I worked backwards down the chain of functions and figured out which ones are used during the normal calculation process, copied them into a modified version of normal_3d_omp.h/hpp and then just changed all the relevant instances of "float" to double. The result is REALLY hacky but it works:

This looks a lot better than what I was getting before, and produces normals that look exactly as I would expect. This is using a search radius of 0.3m

[mattforestyoung]

I can go through and try to validate each stage individually like you suggested, but I think the root cause is definitely the use of floats rather than doubles. So is there value in trying to verify each stage in the process or would it be better to skip straight to a solution?

[taketwo]

We have functions in PCL that support both single and double precision through template parameter, e.g. planeWithPlaneIntersection. Maybe the same can be done with the functions involved in your pipeline?

[mattforestyoung]

I guess that'd be the most straight-forward way to do it, then it's mostly automatic. That solution would essentially involve adding a new declaration of the important functions, at the very least the following:

• computePointNormal
• solvePlaneParameters

Which I assume would also require creating new definitions of the class variables covariance_matrix and xyz_centroid They're defined in normal_3d.h and I had to create double duplicates of those variables to get my hack to work.

In addition, what options would the user have to force the use of the float or double versions? The highest level code the user is going to want to use is the NormalEstimation class. I.e. if this is all the user sees:

pcl::NormalEstimationOMP<PointT, PointT> ne; 
ne.setRadiusSearch(...);
ne.setViewPoint(...);
ne.setInputCloud(...);
ne.compute(...);

How does the underlying code know to use floats or doubles? The effect this bug/issue can have is pretty dramatic (as I'll show in a post below) so I feel like it'd be important to make the user away of the difference and give them the option to force one case or the other.

[mattforestyoung]

So I've been testing matching sequences of 35 clouds, and just incrementally matching each of the 35 back to the first cloud. This bug/issue has a pretty major impact on how well the clouds are matched in some environments. So in all the pictures below, the left hand image is the match BEFORE I fixed this issue, and the right hand image is AFTER fixing it with my hacked code.

In some areas it doesn't make much difference, either because the match was already pretty good or because there isn't much that can improve it:

(NOTE: The pictures are actually pretty big, so click on them for a larger version, rather than squinting at what you see below)

But in other areas it is the difference between a good match and a useless one:

So for my application, using doubles rather than floats makes a world of difference.

[taketwo]

Tentatively, I'd propose the following:

• template computeMeanAndCovarianceMatrix and solvePlaneParameters on the scalar type
• template computePointNormal on the scalar type, at the same time making covariance_matrix_ and xyz_centroid_ local to that function
• add a setter/getter to NormalEstimation to control desired precision
• update computeFeature to take precision into account and use corresponding version of computePointNormal.

[SergioRAgostinho]

I can go through and try to validate each stage individually like you suggested, but I think the root cause is definitely the use of floats rather than doubles. So is there value in trying to verify each stage in the process or would it be better to skip straight to a solution?

We should go straight for the solution. Your time is precious and we should use it efficiently.

How does the underlying code know to use floats or doubles? The effect this bug/issue can have is pretty dramatic (as I'll show in a post below) so I feel like it'd be important to make the user away of the difference and give them the option to force one case or the other.

The user will need to explicitly request double precision through template parameters in case it desires it. In the planeWithPlaneIntersection example Sergey linked before, that it achieved with

planeWithPlaneIntersection<double>(...)

@taketwo we should consider defaulting actually to double, since we often have reports of bad normals. The user who wishes speed then can explicitly take the risk of using single fp precision.

[taketwo]

The user will need to explicitly request double precision through template parameters in case it desires it.

I'd like to clarify that I propose template parameter-based selection of precision class only for free functions. For the NormalEstimation class I think it's better to have a setter/getter such that precision can be adjusted at runtime. Reasons: we don't want to double the amount of instantiations of NormalEstimation due to added class template parameter. Further, I believe it's nice to be able to control precision class through a variable that can be changed at runtime (from command line input or config file) versus alternating code and recompiling.

we should consider defaulting actually to double

Do we understand in which cases single is okay, and in which double is needed? Does in have to do with pointcloud extents? Maybe we can have a heuristic that selects precision automatically if it was not set explicitly.

[SergioRAgostinho]

Do we understand in which cases single is okay, and in which double is needed? Does in have to do with pointcloud extents?

That's the current intuition. Whenever points stray away from 0 too much, we start having problems.

Maybe we can have a heuristic that selects precision automatically if it was not set explicitly.

Hmm. I think I'd rather keep it simple for now. But to provide an informed opinion I need to have an intuition on the time penalty we're incurring for jumping into double precision. Something which reports a time metric per normal point and per the local neighborhood size.

[taketwo]

That's the current intuition. Whenever points stray away from 0 too much, we start having problems.

Well, if this is the only reason, then we should just de-mean point neighborhoods before computing covariance matrix. Otherwise, what if points stray even further away from 0? Provide quadruple precision implementation?