I now ned about 56s for 2000x2000 inverse localization for 3 sensors on
my laptop (Intel Core i5-3320M, 2.60GHz, quad-core, linux, 8GB RAM,
mono-thread test). This means more than 210000 pixels/second.

It's fast!

When ground points are close to first or last pixel, we need to be able
to compute angles sightly after line end, so a small 5 pixels margin has
been added in the search algorithm.