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