Fixed undistort.cpp