Extract less keypoints from maps for faster KD-Tree building

Issue description

At the beginning of Localization step, we extract keypoints from map and build a KD-Tree with them. This KD-Tree is then used to quickly perform nearest neighbors queries during ICP matching step, to get map points close to each current keypoint.

Currently, in order to avoid extracting all keypoints from map, we compute the bounding box of the current frame keypoints, and extract all points from map that lie within this bounding box. The following picture shows in blue the plane keypoints from current frame, and in white the keypoints that were extracted from planes map.

We can see that lots of useless regions are also extracted especially on extreme left and right sides, where we don't have any current keypoint that lies in these regions. By not extracting these useless points, we could greatly decrease the number of keypoints extracted from maps, and therefore accelerate KD-tree building as well as NN queries.

Change suggestion

We could imagine to extract all points from RollingGrid cells where current keypoints lie. But for far and isolated keypoints that are located near the boundaries of the cell, I fear that we will miss neighboring points.