Cleaning up code in the calibration. Still crashes on undistort