Defined kdTree functions. Implemented findMedian.
Merge origin/master