Adapted the code to handle 2D point clouds and decided to split the initial/icp/complete transformation matrices in 3 differents files. It should be easier to post process the transformations.