Added undistort function to show the qualitiy of the result. Also added a save function.