Removed some debug code
More bugfixes in the KD-tree builder