KD-Tree added

- KD-Tree building added
- unittest added