Cleaned the code
Added camera calibration