Naive detection with closest points. (segfault issues)

Uses the nth_element function to get the points with distance <= a specific
element, then takes the mean of these points. Issues with segfaulting, seems to be a memory leak or something, but can't pinpoint it.