Refactor namespaces and other clean up
Clean up kdtree code

Simplifies header includes
More clean up
Clean up