[registration] GICP not estimating rotation transform correctly

[blackccpie]

Describe the bug

When trying to register two pointclouds, GICP seems to correctly estimate the transformation's translation part, but the rotation part always remain close to identity. The final registration has therefore poor result.

Context

Simple pointcloud registration

Expected behavior

On simple cases like the one I want to test, I would expect GICP to perform correct rotation and translation registration.

Current Behavior

GICP seems to correctly estimate the transformation's translation part, but the rotation part always remain close to identity

To Reproduce

Take a simple pointcloud PC1, and another pointcloud PC2 which is just PC1 rotated of a few degrees. Try to register PC1 with PC2 using GICP and default parameters.

Screenshots/Code snippets

pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ,pcl::PointXYZ>::Ptr gicp( new pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ,pcl::PointXYZ>() );

gicp->setInputSource( PC1);
gicp->setInputTarget( PC2 );
gicp->align( *PC1 );

Your Environment

• OS: Windows10
• Compiler: Visual Studio 2017
• PCL Version: 1.9.1

[taketwo]

Did you try standard ICP? How does it perform with your data?

[blackccpie]

@taketwo : the standard point-to-point ICP performs well in my case. The two pointclouds I'm using for my test are available here. These are just the same pointcloud rotated by a few degrees.

Thanks for your help

[blackccpie]

@taketwo : I don't know if you managed to reproduce the problem, anyway I tried to illustrate my results on the following image:

One can see that the green registered pointcloud on the right is far from the target poincloud.

[taketwo]

Unfortunately, I'm not familiar with PCL's implementation of GICP. It could be that there is a bug there. I guess you'll need to dig into the code to understand what's going on and why it's worse than P2P ICP.

[blackccpie]

@taketwo : ok I understand, thanks anyway. I'll try to investigate myself digging into the code, but if any other contributors feel inspired, I'll be glad to have some help.

Regards

[blackccpie]

I found something interesting, it may be related to this line:

https://github.com/PointCloudLibrary/pcl/blob/2b00accccc705ae120af176cd5a0149611863591/registration/include/pcl/registration/impl/gicp.hpp#L209

if I reduce the gradient tolerance to 1e-3, the registration converges to a much nicer solution. I feel like this parameter should be configurable, and at the same time I'm thinking of the mathematical consistency of checking the norm of a 6DoF gradient vector mixing positions and angles:

https://github.com/PointCloudLibrary/pcl/blob/2b00accccc705ae120af176cd5a0149611863591/registration/include/pcl/registration/bfgs.h#L411-L422

[kunaltyagi]

I feel like this parameter should be configurable,

Yes, a PR would be welcome to fix this

I'm thinking of the mathematical consistency of ... mixing positions and angles:

It's not good. Perhaps, we could have 2 tolerances: for linear and for angular error. Personally, I'm not fond of Euler angle based error (Quaternion based error is much more expressive)

[SergioRAgostinho]

Perhaps, we could have 2 tolerances: for linear and for angular error.

Without additional context that's the way to do. It's the most intuitive way.

Personally, I'm not fond of Euler angle based error (Quaternion based error is much more expressive)

Neither. Extract the rotation difference as R = R^T_gt @ R_est and then apply the following

arccos(clamp(0.5 * (trace(R) - 1), -1, 1))

Every 3d rotation can be expressed as a rotation around a given axis of a certain positive angle. If you're literally only interested in the magnitude of the angular error, this is the way to go.

[blackccpie]

@kunaltyagi @SergioRAgostinho : I agree with the point about euler angles. Should we handle this in two steps : first PR to have two separate and configurable tolerances (easy one), and a second one with the axis rotation trick (a bit more work)? I think I could handle these PRs, but that could take some time. @SergioRAgostinho : could you explain a bit more your R = R^T_gt @ R_est expression? Sorry about the naive question but what do ^ and @ operators stand for? Does _gt stands for ground truth?

[kunaltyagi]

could you explain a bit more your R = R^T_gt @ R_est expression?

The trace is \sum_{i,j,k} cos \theta + u_i**2(1-cos \theta) (from the 3d rotation matrix) where \sum u_i**2 = 1. This gives the formula (without the clamp). The clamp is for numerical stability, or in case the matrix was computed with an error. arccos gives the positive angle.

Every 3d rotation can be expressed as a rotation around a given axis of a certain positive angle

That's almost the intuition of quaternions 😄 But Sergio's method is more efficient here.

Should we handle this in two steps

One step should be fine. Configure 2 tolerances along with the angular-error as the angle from trace

EDIT: Found almost the same maths on wikipedia

[blackccpie]

@kunaltyagi : computing the trace of R and the arccos was ok, I was more confused about the R computation : R = R^T_gt @ R_est. But it'ok now, I understood ^T was for transpose, and @ for matrix multiply.

[SergioRAgostinho]

Apologies. It's numpy notation I totally overlooked the explanation :'D But you got it perfectly!

[blackccpie]

The PR is ready. I started thinking about performing the optimisation on the quaternion parameters rather than on the euler angles, but that seems like a non trivial modification. For sure it would be more elegant, but I'm not sure the precision gain will be noticeable.

[taketwo]

R = R^T_gt @ R_est

Sérgio was bilingual here and mixed LaTeX (^T for superscript and _gt for subscript) and Python 3.5+ (@ for matrix multiplication). And yes, strictly speaking, the LaTeX part lacked {} because subscripts are multi-character.