Fixed ICP, used homogenious/4D matrices instead of the builtin affine3d type