search

norlab-ulaval / libpointmatcher

An Iterative Closest Point (ICP) library for 2D and 3D mapping in Robotics
BSD 3-Clause "New" or "Revised" License
1.61k stars 544 forks source link

Fix the surface normals datapoints filter covariance matrix bug #465

Closed aguenette closed 3 years ago

aguenette commented 3 years ago

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 commented 3 years ago

Can one of the admins verify this patch?

pomerlef commented 3 years ago

add to whitelist

YoshuaNava commented 3 years ago

@aguenette so the computed eigenvalues had an unexpected scale?

pomerlef commented 3 years ago

yes exactly. We pass from wrong to dense_mapper_markers2

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

aguenette commented 3 years ago

@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:

SamplingSurfaceNormals_before

and after:

SamplingSurfaceNormals_after

SurfaceNormals before:

SurfaceNormals_before

and after:

SurfaceNormals_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 commented 3 years ago

@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 :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 --> https://github.com/ethz-asl/libpointmatcher/issues/376#issuecomment-870630306
aguenette commented 3 years ago

@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)