Added iterative Closest Point