[Registration] NDT implementation flaws

[kunaltyagi]

Hey, this merged PR is not good.
I noticed this problem because ndt didn't work well in the latest version.

First,the following line is redundant and confusing.
https://github.com/PointCloudLibrary/pcl/blob/master/registration/include/pcl/registration/impl/ndt_2d.hpp#L478

double cos_angle = 0.5 * (transformation_delta.coeff (0, 0) + transformation_delta.coeff (1, 1) + transformation_delta.coeff (2, 2) - 1);

the following code is enough.

double cos_angle = 0.5 * (transformation_delta.coeff (0, 0) + transformation_delta.coeff (1, 1));

or

double cos_angle = transformation_delta.coeff (0, 0);

Secondly, it is wrong to use cos_angle in 3d like as 2d.

https://github.com/PointCloudLibrary/pcl/blob/master/registration/include/pcl/registration/impl/ndt.hpp#L153

double cos_angle = 0.5 * (transformation_.coeff (0, 0) + transformation_.coeff (1, 1) + transformation_.coeff (2, 2) - 1);

Originally posted by @rsasaki0109 in #1724 (comment)

[kunaltyagi]

@rsasaki0109 Could you please give some examples to show the flaws in the existing implementation?

If possible, would you like to create a PR?

[rsasaki0109]

It is a mathematical problem.
The homogeneous transformation matrix H(x,y,θ) in 2d is as below.

So, using the change of cos_ang in ndt_2d.hpp is correct for the convergence judgment of scan-matching algorithms(but redundant) because cos_ang is

But in 3d, H(p) is,using the Euler sequence z-y-x,

where ci = cosφi and si = sinφi.
(reference:page 63 of a thesis about ndt in reserchgate)

So, cos_ang in ndt.hpp is nonsense because it is

[kunaltyagi]

It make sense. Thanks for the explanation.

Would you mind creating a PR? It might take lesser effort for you 😄

[rsasaki0109]

I'm sorry, I misunderstood.

Any rotation matrix R(θ) in three-dimensional space can be represented by a rotation axis r and a rotation angle θ.
And trace{R(θ)} is 2cosθ + 1.
In the convergence judgment of scan-matching algorithms, this theta is considered.

I think that there is an another cause that ndt-scanmatching don't work well in my environment.
I will let you know the details once I find out.

[kunaltyagi]

another cause that ndt-scanmatching don't work well in my environment

Sure. Please share your findings with us once you debug it, even if the issue wasn't in PCL 😄