Iterative Closest Point Open 3d Python Matplotlib