Add tests for kdtree