Updated Point-to-plane Covariance Code

[goldbattle]

In the process of trying to get an accurate covariance from this library I have updated the point-to-plane minimizer to produce it. I was unable to figure out how the original covariance code was derived and the second derivative in respect to the measurement and then state did not make sense. If someone could point me in the direction to get the same equations as the original repo.

This is based off of this work and repository which does symbolic differentiation:

Prakhya, Sai Manoj, et al. "A closed-form estimate of 3D ICP covariance."
2015 14th IAPR International Conference on Machine Vision Applications (MVA). IEEE, 2015.
http://www.mva-org.jp/Proceedings/2015USB/papers/14-27.pdf
https://github.com/saimanoj18/3d-icp-covariance

I re-ran the scripts and also changed it to get a 2d icp covariance using the same methodology.

[ethzasl-jenkins]

Can one of the admins verify this patch?

[pomerlef]

ok to test

[YoshuaNava]

Hi,
This looks like a really nice contribution. Thank you!

I wanted to point out, nonetheless, that I recently read this paper where different methods to estimate ICP Covariance are compared (Censi, CELLO-3D and their own). Their conclusion is that Censi's approach tends to be over-confident, as it ignores the dependence of the ICP final estimate distribution on the initial guess provided, which is a key factor.

The author of the above paper is @mbrossar. He has provided an example implementation here. In the paper he points out that he runs 12 iterations of ICP to sample the covariance properly, but maybe this could be reduced to a lesser but still significant value?

[YoshuaNava]

@goldbattle Do you have any metrics about:

• Usage of computational resources (CPU/memory) of this new covariance estimation method?
• Quality of the estimates.

And an example application of the changes you made to a project? This is quite an interesting contribution and it would be great to look at it closely.

[YoshuaNava]

@goldbattle Do you have any metrics about:

• Usage of computational resources (CPU/memory) of this new covariance estimation method?
• Quality of the estimates.

And an example application of the changes you made to a project? This is quite an interesting contribution and it would be great to look at it closely.

Ping @goldbattle :)

[goldbattle]

I have not performed a very thorough evaluation as in Martin's paper. Censi’s covariance seems to give reasonable results such that I don't need to inflate/deflat it in order to incorporating it into a probabilistic SLAM framework. I have not verified as in Martin's work, so probably in the edge cases it breaks down and is very poor.

As in terms of time, for 2d ICP it takes about 3-4ms single threaded, set using the OMP_NUM_THREADS=1 environmental variable, for alignment of two clouds with 500 points. I have a Intel(R) Xeon(R) CPU E3-1505M v6 @ 3.00GHz. I don't have any comparison to non-covariance based since that isn't a usable result for me.

[YoshuaNava]

@goldbattle I will try your method on 3D point clouds.

Regarding the code, would it be possible to maybe implement a switch? So that we have different methods to estimate the covariance?

[goldbattle]

I am not even sure how the old results were derived. If someone can point me to the derivation then that would be ideal, as I didn't get anything close to what was implemented for the small parts that I did original derive by hand.

[pomerlef]

I'm 100 % sure, but it might be from here : https://arxiv.org/pdf/1410.7632.pdf
but the math are from here: https://www.researchgate.net/publication/276269511_A_Closed-form_Estimate_of_3D_ICP_Covariance

But, that's on top of my head. I might be wrong.

[YoshuaNava]

Update: I have not forgotten about this PR, but I'm currently very busy. I'm in contact with @goldbattle to introduce a way of testing this PR and others that might change the way covariances are computed.

I estimate that I work on this again in April.

[pomerlef]

It's been a while, where are we with this?

[YoshuaNava]

@pomerlef Sorry for the delay. I'll look into this very soon.

I plan to bring back the original covariance code and allow the user to choose between the old and new method. Hopefully also implement a basic unit tests. Do note I don't know how to verify covariances in testing code, if you have any ideas or references I would be happy to take a look.

[pomerlef]

Unfortunately, validating a covariance is still ill defined (at least for my personal point of view). In these cases, it might be better to check what is current output and making sure that it's not influenced in the future by an external modification. That's how the current filters are validated in terms of impact on ICP.