KdTree operationnal
Some non-important clean up