Support singular ICP steps in PointToPlane(Cov)ErrorMinimizer

[HannesSommer]

Unfortunately it is not clear to me what the right solution for this problem is (see https://github.com/ethz-asl/libpointmatcher/compare/fix/supportSingularICPStepsInPointToPlaneErrorMinimizer?expand=1#diff-51b8a83999c74513d51e6839db229186R188).

Both suggested ways work nicely for me, though. Maybe a priority (user provided) based algorithm could be the right way.

In https://github.com/ethz-asl/libpointmatcher/compare/fix/supportSingularICPStepsInPointToPlaneErrorMinimizer?expand=1#diff-51b8a83999c74513d51e6839db229186R207 there as a new exception introduced for the case that this does not work. It should cover possible implementation failures. In particular it could be that I guessed wrongly when filling in the specification gaps of Eigen's FullPivHouseholderQr algorithm.

[HannesSommer]

Something was confusing me about the current algorithm when writing this patch:
In https://github.com/ethz-asl/libpointmatcher/blob/master/pointmatcher/ErrorMinimizersImpl.cpp#L348 there is the llt() used to solve Ax =b and I didn't touch that here (just moved and deduplicated it).

This is confusing me:

1. I thought LLT based solving doesn't make sense (if defined at all) for non symmetric real matrices and
2. at that line A doesn't look like symmetric in general (and at least for singular cases a can confirm that it isn't)

Is it some magical detail about point to plain ICP that it still makes sense to use LLT here?

[pomerlef]

ok, there is a lot of action on the pull request this week.

Could you put me in context of what is the "singular ICP steps" problem. I lost track of what it is and when it happen.

As for the llt() call, the original derivation of ICP we finished with something like G_G.transpose() x = G_h (see eq. 2.1, p. 44 here) which should give a symmetric matrix (I think). I'm most probably missing something if you don't have a symmetric matrix on your side.

[HannesSommer]

This PR has time, I guess.
It is entirely about this todo / former exception : https://github.com/ethz-asl/libpointmatcher/blob/master/pointmatcher/ErrorMinimizersImpl.cpp#L330.

This case can happen if two point clouds do not determine certain aspects of the transformation for registration (as e.g. when the reference cloud is planar; as in the added unit test).

About the symmetry of A in https://github.com/ethz-asl/libpointmatcher/blob/master/pointmatcher/ErrorMinimizersImpl.cpp#L326: I'm sorry for the confusion, you're right. It is symmetric :).

I read the definition of wF in A = wF * F^T wrongly. In fact it is wF = F * W with the diagonal weighting matrix W. I thought of W * F (probably mislead by the name) and that would make A asymmetric.

Btw, if this is true one could get rid of the entire wF matrix by writing
A = F * mPts.weights.asDiagonal() * F.transpose() and be even faster and use half the memory.

I believe, one could make it even faster and get entirely rid of F and wF (if I'm not mistaken):
A = [ C*W*C^T C*W*N^T ; (C*W*N^T)^T N*W*N^T ]
because F = [ C ; N ].

I could try that with another PR. Would that be useful?

[pomerlef]

Awesome, I could correct Hannes on some math stuff. I'll have to put that in my CV now ;)

Everything that can speed up the computation is welcome. If it start to be too compact to be readable, please add comment to explain what is going on. It would be very good if the notation could follow a bit the document I sent you, so I can understand what is going on.

As for this PR, it looks good to me. Do you have any more concerns or we are ready to go?

[HannesSommer]

:). Yes, this PR is ready from my side.

[pomerlef]

Thanks Hannes