Cleaning up the undistort code