2-dimensional ICP registration in MATLAB