Fixed a bug and added image undistortion

Final commit