Focussing with a focus pyramid
Once your focus pyramid is calibrated, you measure the distances of the four sub-images and read of the focus position of your distance-to-focus calibration. The calibration will work with either sides or diagonals of the images; try to measure as close to the expected focus as possible and consider that the optical path with pyramid will be longer than without, so best compensate this with an additional optical lense. The method is robust enough, though, that you can also work with subimages that are slightly out of focus, or, if your focus positioning is reliable, you can add an offset to get the sub-images in focus and consider it accordingly once the distances are measured.
Get the pyramidal images half-diagonal distance by localizing the minimum (f=33.92mm) in the plot above to d=31.5 px.
The half-diagonal of the pyramidal images is an excellent proxy for the focus. Once calibrates to f=33.92mm, and measuring the slope of the distance-to-focus line, a single measurement of the distance in the pyramid images yields the optimal focus.
Calibration of sub-image distances to focus has to be done once, again using, e.g., FWHM minimization. First, note that the focus position to sub-image distance is an almost perfect linear relation. Once the slope of this line has been established, a single measurment at any focus position can deliver the offset to the true optimal focus. Calibration of the focal line should be done on a close-to perfect night, though, as errors in the calibration will lead to systemmatic errors in the focus.