[core] Bug with incorrect finding has been fixed (issue #113); Unit-tests have been added for KD Tree