Rigid correspondence calculation complete. Added ICP utility.