Clean KDTree building code.

Refactored the build code, with new proper variable names and some
simplifications.
Also the dimension to split on is calculated as the variance in the
dimension.