[registration] Distribution-to-Distribution NDT (D2D-NDT)

[HuaTsai]

Hello developers,

I want to ask your opinions on implementing Distribution-to-Distribution NDT (D2D-NDT).

The current NDT version in PCL is P2D-NDT proposed by Magnusson in Ph.D. thesis. The concept of D2D-NDT was introduced ten years ago in 2012 by Stoyanov et al., please refer to the paper here. According to the paper, the benefits of using D2D-NDT instead of P2D-NDT are:

1. Faster running time: changing correspondences from point to cell to cell to cell, which reduces a large amount of time.
2. Comparable accuracy with P2D-NDT
3. The better success rate for scan matching

In my practice, 1 always happens due to the mathematical background. However, I have not fully investigated about 2 and 3, and they may depend on the data quality.

If this feature looks good to you, I would like to create a pull request for the early draft accordingly. The PR will contain three new files: ndt_d2d.h, ndt_d2d.hpp and ndt_d2d.cpp. I believe that there are many details to discuss and review, looking forward to the feedback then.

Best, HuaTsai

[mvieth]

Hi, thanks for the idea. I hadn't heard of D2D-NDT before, but I quickly read over the paper. The paper mentions that an open-source ROS package is available. Do you know anything about that? Especially how well it is maintained? Are you planning to only add the D2D-NDT, or both D2D-NDT and the global descriptor for initial alignment? Also: can you give an estimate whether this can be merged with the existing NDT implementation (P2D-NDT)? As far as I understood, both variants have common things, e.g. the More-Thuente line search. It would be important to avoid code duplication. Maybe it is even possible to integrate the D2D-NDT completely into the existing P2D-NDT, so users only have to set one option to choose between P2D-NDT and D2D-NDT? Looking forward to your response, and thanks again for the proposal.

[HuaTsai]

mvieth, thank you for your kind reply

1. The author's ros package is not under maintenance since a long time ago. You can check the D2D-NDT source code here. Honestly, the source code is not friendly for reading and modifying and is more likely for academic purposes. I have tried it a few times and started to write a new one, taking their codes as references.
2. I am planning to add the D2D-NDT only. I am not familiar with the descriptors.
3. I agree that it will be better to incorporate D2D-NDT into the existing P2D-NDT code. I think it is feasible and may try out this direction. But for readability and consistency, I need to modify some namings/declarations of variables, mainly used in computing gradient and hessian, and then incorporate D2D-NDT. I will try to provide sufficient details on the differences.

[mvieth]

I have tried it a few times and started to write a new one, taking their codes as references.

Please make sure you don't do anything that violates their copyright. If you, for example, copy large portions of their code, we likely can't merge that into PCL.

I am planning to add the D2D-NDT only. I am not familiar with the descriptors.

Alright, that's fine.

But for readability and consistency, I need to modify some namings/declarations of variables, mainly used in computing gradient and hessian, and then incorporate D2D-NDT. I will try to provide sufficient details on the differences.

Please note that we can't easily change names of functions or variable that are accessible to PCL users. Feel free to open a pull request with your proposed changes at any time. However please understand that I might request a lot of changes since I don't know what you are going to propose. You could roughly describe your planned changes here first, then I can give you my thoughts before you spend too much time on something.

[HuaTsai]

I tried to implement D2D-NDT to be compatible with PCL, but the result failed. My current code cannot converge to the correct transformation. Because of this behavior issue, I tend not to create the PR now. My previous successful experience is using ceres-solver, and assigning the gradient expression.

If you could help, please check my forked codes ndt.h and ndt.hpp. I found that the computed Hessian is too small, and wonder if this is the reason. Also, I prepare a document of computing gradient and Hessian, please check pdf document. Note that the Hessian representation in the original paper is incorrect.

I originally planned to implement it before starting to fulfill mandatory military service. But it seems that I am too positive. Sorry for that I could not check this issue regularly since I am in mandatory military service now, and could only reply on the weekend.

[HuaTsai]

Let me close this issue since I don't have time to code. I may reopen it when I have time and make the algorithm work. Sorry for the inconvenience caused. 😓