Working float/double templatized KDTree and unit test