Support three degrees of freedom

[caobingquan6]

Can we support gicp registration with three degrees of freedom of x, y and yaw for 3D point sets?

[koide3]

We do not officially support 3DoF estimation, but to fix XY rotation and Z translation, I think you can constrain the optimization by adding alpha * diag([1, 1, 0, 0, 0, 1]), where alpha is a large number (e.g., 1e6), to the Hessian matrix at the following line.

https://github.com/SMRT-AIST/fast_gicp/blob/aff82ee0778178b8942e2d4d31855c0d8421ac3b/include/fast_gicp/gicp/impl/lsq_registration_impl.hpp#L128

[caobingquan6]

What do you mean is that we need to change

double y0 = linearize(x0, &H, &b);

to:

double y0 = linearize(x0, &H, &b);
double diag=1e6;
H(0,0)+=diag;
H(1,1)+=diag;
H(5,5)+=diag;

Is that right? And what's the principle? I think the H matrix only represents the second gradient of the objective function, but how does it fix XY rotation and Z translation？

Looking forward to your reply. Thank you very much!

[koide3]

Yes, it's right.
By adding a large value to a diagonal element of the Hessian matrix, the update vector of that dimension becomes small, and the estimate tries to stay at the current solution. The principle is the same as how the damping factor (lambda) of LM optimization works.

[caobingquan6]

Thank you very much for your reply! I have another question to ask.

We can see that

double y0 = linearize(x0, &H, &b);

,that is, y0,b and the calculated H matrix correspond to each other. So if we only change the value of H matrix after the above equation (using H+=alpha * diag([1, 1, 0, 0, 0, 1])), while the values of y0 and b do not change, will this be a problem? And Is it theoretically correct?

[koide3]

It doesn't matter. It can be interpreted as introducing a prior constraint that penalizes the displacement from the current estimate in the theoretical standpoint. It's often used to stabilize the optimization (as in LM optimization) and to fix gauge freedom (e.g., http://ais.informatik.uni-freiburg.de/teaching/ws11/robotics2/pdfs/ls-slam-tutorial.pdf).