works for constant focal
now working with calibration data