Camera Calibration code

Replaced all code with standard Camera Calibration code.