ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Require Help Deriving correct formula to calculate object distance

asked 2023-06-12 21:06:15 -0500

distro gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2023-06-13 05:28:16 -0500

sohai gravatar image

When accessing pixel values in an image in OpenCV, the convention is image[row][col] or image[y][x]. Currently it is [x][y] and that may be why you are getting the error (since you are accessing non-existent pixels).

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2023-06-12 21:06:15 -0500

Seen: 229 times

Last updated: Jun 13 '23