Calibration kind of working

Rough calibration and depth reconstruction (works ok, error a bit large)