Iterative Closest Point Python