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

# Require Help Deriving correct formula to calculate object distance

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 ...
edit retag close merge delete

## 1 Answer

Sort by ยป oldest newest most voted

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).

more

## Stats

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

Seen: 229 times

Last updated: Jun 13 '23