Fixing the calib pose calculation and usage