faster tree heap when less neighbors than requested
Removed useless optimisation, cleaned-up.
Merge branch 'master' of git://github.com/ethz-asl/libnabo into HEAD