Support other map sampling techniques

[boxanm]

If I recall correctly, the sampling technique, that's currently being applied to the map works as follows (suppose the scan point cloud is in the map frame):

if the map is empty: 
      map <-- scan
else: 
      Iterate over all points in the scan:
            If the distance between the scan point and its closest neighbour in the map is > epsilon:
                  Save the scan point's index. 
      Concatenate all the scan points with saved indices to the map.

This approach has two flaws. Firstly, no subsampling is performed on the first scan. Secondly, the distance condition is not checked against scan points from the current iteration.

The reason for this implementation is that the point clouds are stored as a kd-tree. In general, the insert operation to a kd-tree is quite costly. Furthermore, libnabo, the library used behind, does not support it at all.

I showed in my master's thesis that at least in some environments, the current approach (in the thesis called Global Max density filter) lacks in terms of map quality and localisation performance. I think that the filter should be replaced by another one (such as Octree-based voxel or Max density), or ideally, it should be fully controllable by the user. This change should be relatively easy since it only requires another call of libpointmatcher's filter. Nevertheless, it requires the option to specify the coordinate frame the filter is applied in, see the other issue #10.