Fix the surface normals datapoints filter covariance matrix bug

[aguenette]

There was a bug in the computation of the covariance matrix, which had an impact in the eigen values and eigen vectors computation.

In the SurfaceNormals filter: the covariance matrix needed to be divided by realKnn.
In the SamplingSurfaceNormals filter: the covariance matrix needed to be divided by colCount.

Credits go to @pomerlef.

[ethzasl-jenkins]

Can one of the admins verify this patch?

[pomerlef]

add to whitelist

[YoshuaNava]

@aguenette so the computed eigenvalues had an unexpected scale?

[pomerlef]

yes exactly. We pass from

to

@YoshuaNava do you know who handle Jenkins at ASL? They changed something and the CI pipeline is broken again.

[aguenette]

@pomerlef I need to rectified something about the pictures you posted. The first one was taken using the SamplingSurfaceNormals filters using 30 knn, a ratio of 0.5 (I think? 🤔) and the square root of the eigen values was not taken (after that it is). The second one used the SurfaceNormals filters with 15 knn (this one I'm sure 😉).

I made a mistake by assuming that both filters were giving the same results. So, after seeing the question of @YoshuaNava yesterday, I ran some tests with both filters before and after the correction were applied.

So, here's some pictures taken from those tests that are more representatives:

SamplingSurfaceNormals before:

and after:

SurfaceNormals before:

and after:

15 knn was used for both filters and a ratio of 1 was used for the SamplingSurfaceNormals filter.

So, it's looks like that the SamplingSurfaceNormals was worst than the SurfaceNormals filter and that it's still problematic even after applying the correction.

Sorry about that. 😓

[YoshuaNava]

@pomerlef Sorry for the delay, I was focused on a software release sprint. At the moment I'm not aware of who is handling the CI from ASL.

@aguenette Thank you for the very detailed explanation 🙂. I have two questions that are closely related:

1. Have you observed any change in the performance (accuracy, robustness, snappiness) of ICP after applying the fix?
2. Which tool do you use for plotting normals? This question is relevant for --> Organized point clouds #376 (comment)

[aguenette]

@aguenette Thank you for the very detailed explanation slightly_smiling_face. I have two questions that are closely related:

1. Have you observed any change in the performance (accuracy, robustness, snappiness) of ICP after applying the fix?
2. Which tool do you use for plotting normals? This question is relevant for --> [Organized point clouds #376 (comment)](https://github.com/ethz-asl/libpointmatcher/issues/376#issuecomment-870630306)

@YoshuaNava, You're welcome! 🙂

1. This fix shouldn't affect the performance of ICP because it only affect the scaling of the eigen values, i.e. the eigen vectors stay the same, and only the smallest eigen value determine which eigen vector is the normal.

2. The above pictures were taken using rviz Marker display type with a MarkerArray message. (https://wiki.ros.org/rviz/DisplayTypes/Marker)