GICP: add Newton optimizer

[mvieth]

It replaces the current BFGS optimizer, which is still available via useBFGS.
The Newton optimizer uses the hessian matrix of the function, while BFGS only computes an approximation of the hessian.
In direct comparison (same GICP cost function to minimize and same starting point/same initial transformation), the Newton optimizer (estimateRigidTransformationNewton) takes only about one third of the time needed by the BFGS optimizer (estimateRigidTransformationBFGS). Every time, the Newton optimizer finds a slightly better transformation (lower cost) than the BFGS optimizer. Note: GICP.align() as a whole becomes only a bit faster (not 3x faster) because it also spends time on nearest-neighbour search.
Even in very difficult situations, e.g. when both point clouds are far away from the origin meaning even a small rotation change results in a large point-to-point distance change, the Newton optimizer still finds a good solution, while the BFGS optimizer often fails completely.

[larshg]

I have tried reading up on the BFGS vs newton method and I find it odd that most of them mention that the BFGS should be faster computationally, since it only approximates the hessian rather than calculating it fully as the newton method, but still you say that the newton implmentation is faster?

I guess it comes down to a better / optimized implementation, rather than the algorithms?

I haven't directly worked with these optimizations functions, so I can only judge more from an implementation view, but it seems good 👍

Should we have a second unit test, where the BFGS algorithm is used or is it covered in other tests?

[mvieth]

I am not sure whether it is possible to say that one of the methods is always better than the other. Newton has the advantage that is has more information about the function to optimize since it uses the exact Hessian. This is probably the reason why Newton needs fewer iterations than BFGS until convergence, because the Hessian approximation of BFGS is seemingly not so good for our objective function.
However, Newton has to compute the inverse (or a decomposition) of the Hessian matrix, which is very slow if the matrix is big. But because we only have six variables to optimize, that is not a problem in our case.
There is also the question how long it takes to compute the Hessian. For some objective functions, this may take very long, but in our case, calculating the gradient+hessian takes only roughly twice as long as calculating only the gradient. Calculating gradient and hessian together in one function (dfddf) is also faster than than calculating them in two separate functions, because many values can be reused.
So which one is better/faster probably depends on the specific objective function (how many variables to optimize, how long does it take to compute the exact Hessian, how good is the approximation by BFGS, ...).

Should we have a second unit test, where the BFGS algorithm is used or is it covered in other tests?

No, currently only the Newton method is tested since it is the new default. But I can duplicate the existing GICP test and make it run with BFGS.

[larshg]

Ah, yeah makes sense.