major kdtree refactoring, start meanshift testcase