OpenCV & Python - How to convert pixel to millimeters (get object coordinates from arbitrary origin)

Question

I am writing a script to take pictures with a fixed camera and calculate the distance between the center of each object in the picture and an arbitrary origin (around the center of the camera). Each picture that the camera takes goes through an "undistortion" script that applies the calibration values (obtained with a modified calibration script and saved to a yaml file). The distance values are off from what I can measure with a caliper.

Code snippet down here. The original code is longer, I only left the parts that capture and handle the image; the VideoCapture class was taken from a post online.

import time
import cv2
import numpy as np
import yaml
import imutils
from imutils import contours, perspective
from scipy.spatial import distance
import threading
import queue
import datetime

class VideoCapture:
  def __init__(self, name):
    self.cap = cv2.VideoCapture(name, cv2.CAP_DSHOW)
    self.q = queue.Queue()
    t = threading.Thread(target=self._reader)
    t.daemon = True
    t.start()

  def _reader(self):
    while True:
      ret, frame = self.cap.read()
      if not ret:
        break
      if not self.q.empty():
        try:
          self.q.get_nowait()
        except queue.Empty:
          pass
      self.q.put(frame)

  def read(self):
    return self.q.get()

def readAndUndistort(cap, cal_par): #cal_par = [mtx,dist,w,h]
    raw_img = cap.read()

    newcameramtx, roi = cv2.getOptimalNewCameraMatrix(cal_par[0], cal_par[1], (cal_par[2],cal_par[3]), 1, (cal_par[2],cal_par[3]))
    mapx, mapy = cv2.initUndistortRectifyMap(cal_par[0], cal_par[1], None, newcameramtx, (cal_par[2],cal_par[3]), 5)
    return cv2.remap(raw_img, mapx, mapy, cv2.INTER_LINEAR, cv2.BORDER_CONSTANT)

def midpoint(ptA, ptB):
    return (float((ptA[0] + ptB[0])) * 0.5, float((ptA[1] + ptB[1])) * 0.5)

def calcPixelsPerMM(img, height_real, width_real = None): #Currently not in use, I applied a fixed value
    width_real = height_real if width_real is None else width_real

    search_area = img[int(roi[1]):int(roi[1]+roi[3]),  
                int(roi[0]):int(roi[0]+roi[2])]
    
    gray = cv2.cvtColor(search_area, cv2.COLOR_BGR2GRAY)
    gray = cv2.GaussianBlur(gray, (7, 7), 0)

    edged = cv2.Canny(gray, 0, 80)

    cnts, _ = cv2.findContours(edged.copy(), cv2.RETR_EXTERNAL,
        cv2.CHAIN_APPROX_SIMPLE)
    c = cnts[0]

    box = cv2.minAreaRect(c)
    box = cv2.boxPoints(box)
    box = perspective.order_points(box)

    (tl, tr, br, bl) = box
    (tltrX, tltrY) = midpoint(tl, tr)
    (blbrX, blbrY) = midpoint(bl, br)

    (tlblX, tlblY) = midpoint(tl, bl)
    (trbrX, trbrY) = midpoint(tr, br)

    height_px = distance.euclidean((tltrX, tltrY), (blbrX, blbrY))
    width_px = distance.euclidean((tlblX, tlblY), (trbrX, trbrY))
    print(f'Height_px: {height_px}, Width_px: {width_px}')

    return height_px/height_real, width_px/width_real

def findOrigin(img):
    global ORIG
    search_area = img[int(roi[1]):int(roi[1]+roi[3]),  
                int(roi[0]):int(roi[0]+roi[2])]
    
    gray = cv2.cvtColor(search_area, cv2.COLOR_BGR2GRAY)
    gray = cv2.GaussianBlur(gray, (7, 7), 0)
    edged = cv2.Canny(gray, 0, 80)


    cnts, _ = cv2.findContours(edged.copy(), cv2.RETR_EXTERNAL,
        cv2.CHAIN_APPROX_SIMPLE)
    c = cnts[0]

    box = cv2.minAreaRect(c)
    box = cv2.boxPoints(box)
    box = perspective.order_points(box)

    #(tl, tr, br, bl) = box
    #ORIG = tl
    ORIG = getCentroid(c)
    cv2.line(search_area, np.array(ORIG, dtype="int"), (int(ORIG[0])+100, int(ORIG[1])), (0, 0, 255), 1)
    cv2.line(search_area, np.array(ORIG, dtype="int"), (int(ORIG[0]), int(ORIG[1])+100), (0, 255,0), 1)
    #print(ORIG)
    return search_area
    


def getCoords():
    undist = readAndUndistort(cam, [mtx, dist, w, h])
    search_area = undist[int(roi[1]):int(roi[1]+roi[3]),  
                  int(roi[0]):int(roi[0]+roi[2])] 
    gray = cv2.cvtColor(search_area, cv2.COLOR_BGR2GRAY)
    gray = cv2.GaussianBlur(gray, (7, 7), 0)
    edged = cv2.Canny(gray, 0, 80)
    edged = cv2.dilate(edged, None, iterations=1)


    cnts = cv2.findContours(edged.copy(), cv2.RETR_EXTERNAL,
        cv2.CHAIN_APPROX_SIMPLE)
    cnts = imutils.grab_contours(cnts)

    (cnts, _) = contours.sort_contours(cnts)

    orig = search_area.copy()
    cv2.line(orig, np.array(ORIG, dtype="int"), (int(ORIG[0])+100, int(ORIG[1])), (0, 0, 255), 1)
    cv2.line(orig, np.array(ORIG, dtype="int"), (int(ORIG[0]), int(ORIG[1])+100), (0, 255,0), 1)

    if len(cnts) > 1:
        for c in cnts:
            if cv2.contourArea(c) < 100 and cv2.contourArea(c) > 150000:
                continue

            box = cv2.minAreaRect(c)
            box = cv2.boxPoints(box)
            box = np.array(box, dtype="int")
            box = perspective.order_points(box)
            (tl, tr, br, bl) = box


            M = cv2.moments(c)
            cx = cy = 0

            if M['m00'] != 0:
                cx = int(M['m10']/M['m00'])
                cy = int(M['m01']/M['m00'])
                cv2.circle(orig, (cx, cy), 3, (0, 0, 255), -1)
            
            posAdjX = -1 if cx < ORIG[0] else 1
            posAdjY = -1 if cy < ORIG[1] else 1
            #print(posAdjX, posAdjY)

            (coyX, coyY) = midpoint(ORIG, (ORIG[0],cy))
            (coxX, coxY) = midpoint(ORIG, (cx,ORIG[1]))

            tolerance = 10

            if ORIG[0] - tolerance <= tl[0] <= ORIG[0] + tolerance and ORIG[1] - tolerance <= tl[1] <= ORIG[1] + tolerance:
               continue

            cv2.drawContours(orig, [box.astype("int")], -1, (0, 255, 0), 2)
            cv2.line(orig, np.array(ORIG, dtype="int"), (int(ORIG[0]),cy),(0, 255,0), 2)   
            cv2.line(orig, (int(ORIG[0]),cy), (cx,cy),(0, 255, 0), 2)    
            cv2.line(orig, np.array(ORIG, dtype="int"), (cx, int(ORIG[1])),(0, 0, 255), 2)   
            cv2.line(orig, (cx, int(ORIG[1])), (cx,cy),(0, 0, 255), 2)   

            dCOx = distance.euclidean(ORIG, (cx, ORIG[1]))
            dCOy = distance.euclidean(ORIG, (ORIG[0], cy))

            dimCOx = float(dCOx)/float(pixelsPerMMx) * posAdjX
            dimCOy = float(dCOy)/float(pixelsPerMMy) * posAdjY

            cv2.putText(orig, f"({int(dimCOx)}, {int(dimCOy)})",(cx, cy), cv2.FONT_HERSHEY_SIMPLEX,0.65, (255, 255, 255), 2)
            cv2.imshow("Result", orig)
            cv2.waitKey(1000)
            cv2.destroyWindow("Result")


            return dimCOx, dimCOy
    else:
        return None


cam = VideoCapture(0)
[mtx, dist, w, h] = readCalib("camera_calib.yaml")


roi_img = readAndUndistort(cam, [mtx,dist,w,h])
roi = ROISelection(roi_img, "Seleziona area di ricerca")
pixelsPerMMy = pixelsPerMMx = 2.075 #fixed value since it doesn't change much between runs
def_img = findOrigin(roi_img)

x,y = getCoords()

This conversion algorithm is very simple, which I suspect is also why it is so inaccurate. There must be a better way to calculate distances (and therefore coordinates) from the origin.

EDIT: here are the camera matrix and distortion coefficients I get from cv2.calibrateCamera and I then use to undistort the pictures. They were taken from the yaml file I save them to (converted from numpy arrays to lists). Width is 640 and height 480.

camera_matrix:
- - 733.0876152107618
  - 0.0
  - 324.4980857211455
- - 0.0
  - 732.2745256469318
  - 262.8575866011689
- - 0.0
  - 0.0
  - 1.0
dist_coeff:
- - -0.4026036749641984
  - 0.2837514169706547
  - 0.0007766050126771588
  - 0.00011414032785012568
  - -0.011067012205441455

This is a sample image with the origin in the middle and the objects I'd like to get the coordinates for.