Require Help Deriving correct formula to calculate object distance

edit

I am trying to create a python scripted node which calculates object distance based on what is detected from an object detection library I'm utilizing. The node is subscribed to the /camera/color/image_raw which it obtains the color image. This data is fed to the object detection model and the bounding box information is obtained. The location of the bounding box center is calculated using formula:

Center_x = x + (width / 2)
Center_y = y + (height / 2)

where:

    (x, y) are the coordinates of the top-left corner of the bounding box.
    width is the width of the bounding box.
    height is the height of the bounding box.

This coordinate is converted to it's corresponding location in the depth image (The node is also subscribed to the camera\depth\image_raw topic). This is the conversion formula I used:

scale_factor_x = width_depth / width_color
scale_factor_y = height_depth / height_color

x_depth = int(x_color * scale_factor_x)
y_depth = int(y_color * scale_factor_y)

Here is the code:

#!/usr/bin/python3
import rospy
from rospy.numpy_msg import numpy_msg
import numpy as np
from ultralytics import YOLO
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
import threading
from std_msgs.msg import Header
import cv2
#from yolov3.cfg import *


class camera_detect:
    def __init__(self):
        self.bridge = CvBridge()
        self.model = YOLO('yolov5s.pt')
        self.mutex = threading.Lock()
        self.cv_image = None
        self.result = None
        self.class_names = None
        self.boxes = None
        self.info = None
        self.distance = None
        self.c1_time_stamp = None
        self.c2_time_stamp = None
        self.Center_x = None # x coordinate of bounding box center
        self.Center_y = None # y coordinate of bounding box center
        self.color_image_height = None #Height of opencv color image
        self.color_image_width = None #Width of opencv color image
    def callback1(self,msg):
        image=msg
        self.c1_time_stamp=msg.header.stamp

        with self.mutex:
            print("callback1 gets lock")
            if self.distance is None:
                self.cv_image = self.bridge.imgmsg_to_cv2(image, desired_encoding='passthrough') #convert to opencv format
                self.color_image_height = self.cv_image.shape[0]
                self.color_image_width = self.cv_image.shape[1]
                self.result = self.model(self.cv_image, verbose=True)[0]
                self.class_names = self.result.names
                self.boxes = self.result.boxes.data.cpu().numpy()

                try:
                    for i in range(len(self.boxes)):
                        print("NUMBER: ",i)
                        class_index = int(self.boxes[i][5])
                        class_name = self.class_names[class_index]
                        print("CLASS_NAME: ",str(class_name) )
                        if  str(class_name) == "fire hydrant" or str(class_name) == "chair" or str(class_name) == "laptop":
                            self.info = self.boxes[i][0:4]
                            self.Center_x = self.info[0] + (self.info[2] / 2)
                            self.Center_y = self.info[1] + (self.info[3] / 2)
                        else:
                            self.info = None

                except IndexError:
                    pass
            print("callback1 releases lock")

    def callback2(self, msg):
        depth_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough') #convert to opencv format
        self.c2_time_stamp = msg.header.stamp

        #SCALE FACTOR RELATES COLOR IMAGE AND DEPTH IMAGE
        scale_factor_x = depth_image.shape[1] /  self.color_image_width
        scale_factor_y = depth_image.shape[0]  /self.color_image_height
        with self.mutex:
            print("callback2 gets lock")
            if self.info is not None:
                #RELATE THE LOCATION OF PIXEL OF CENTER OF BOUNDING BOX TO PIXEL LOCATION IN DEPTH IMAGE
                x_depth = int(self.Center_x * scale_factor_x)
                y_depth = int(self.Center_y  * scale_factor_y)
                print("X_DEPTH: ",x_depth,"Y_DEPTH: ",y_depth)
                self.distance = depth_image ...