Clean up KDTree
This MR cleans up our implementation of KDTree
for closest neighbor search
- removed from objects, will never be stored to file
- implemented as template class to allow whatever objects we want in the trees
- remove raw pointers in the class, remove necessity to implement special destructor
- in tracking, use them directly instead of pointers to avoid having to clean them up manually