Задача №2 Вычисление расстояния до объекта при неизвестных габаритах

Следующий код детектирует объект круглого, красного цвета и рассчитывает расстояние до него с помощью RGBD данных.

В данном кейсе мы заранее не знаем реальных размеров объекта.

Разберем код детальнее, его можно найти по пути: /catkin_ws/src/applied_robotics-trackingcam3d_client_ros/src/recognition_object.py

Это полноценный ROS-узел, который:

Подписывается на RGB-камеру, depth-камеру и параметры калибровки камеры.
Находит круглый объект на RGB-изображении.
Извлекает глубину из области, соответствующей объекту.
Вычисляет расстояние и 3D-координаты.
Отображает маркер-сферу в RViz.
Печатает расстояние до объекта.

Импортируются ROS-библиотеки, OpenCV, NumPy, и средства для преобразования между sensor_msgs/Image и cv2:

import rospy, cv2, numpy as np, message_filters

from sensor_msgs.msg import Image, CameraInfo

from visualization_msgs.msg import MarkerArray, Marker

from geometry_msgs.msg import Pose

from cv_bridge import CvBridge

1.Класс Get_distance_from_camera:

self.marker_array_msg = MarkerArray()

marker = Marker()

self.marker_array_msg.markers.append(marker)

Создаётся массив маркеров (MarkerArray) с одним пустым Marker, который позже будет переиспользоваться.

Подписка на топики:

self.image_sub = message_filters.Subscriber(... image_raw)

self.depth_sub = message_filters.Subscriber(... depth/image_rect)

self.camera_info_sub = message_filters.Subscriber(... camera_info)

Используется message_filters.ApproximateTimeSynchronizer, чтобы вызывать callback(...) только когда все три сообщения пришли синхронно.

self.pub = rospy.Publisher('/recognition_object', Image, ...)

self.publisher_marker = rospy.Publisher('/visualization_marker_array',
MarkerArray)

Один публикует изображение с результатами, другой — массив маркеров в RViz.

Основная логика:

Калибровочные параметры:

m_fx = camera_info.K\[0\] \# фокус по X

m_cx = camera_info.K\[2\] \# центр X

m_fy = camera_info.K\[4\] \# фокус по Y

m_cy = camera_info.K\[5\] \# центр Y

Используются для перевода 2D координат из изображения в 3D.

cv_rgb = self.bridge.imgmsg_to_cv2(rgb_data, "bgr8")

depth_image = self.bridge.imgmsg_to_cv2(depth_data, "32FC1")

Преобразование ROS-изображений в OpenCV-формат.

depth_image_re = cv2.resize(depth_image, (640, 480))

depth_array = np.array(depth_image_re, dtype=np.float32)

Преобразование глубины и нормализация в формат визуализации.

gray = cv2.cvtColor(cv_rgb, cv2.COLOR_BGR2GRAY)

circles = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT, ...)

Находит окружности методом Хафа.

roi_depth = depth_image_re[y-r:y+r, x-r:x+r]

Извлекается область глубины вокруг окружности.

for p in range(0, roi_depth.shape[0]):

for l in range(0, roi_depth.shape[1]):

value = roi_depth.item(p, l)

if value > 0:

sum += value

n += 1

Вычисляется средняя глубина (исключая нули).

point_z = mean_z * 0.001

point_x = ((x + 320) - m_cx) * point_z * inv_fx

point_y = ((y + 240) - m_cy) * point_z * inv_fy

Из пиксельных координат и глубины рассчитывается 3D-позиция в системе координат камеры.

Построение и публикация маркера:

marker = Marker()

marker.header.frame_id = "stereo_camera"

marker.type = Marker.SPHERE

marker.pose.position.x = point_x

marker.pose.position.y = point_y

marker.pose.position.z = point_z

Создаётся сфера в RViz, соответствующая обнаруженному объекту.

self.marker_array_msg.markers[0] = marker

self.publisher_marker.publish(self.marker_array_msg)

Сфера публикуется в RViz.

Объединение и публикация изображений:

rgbd = np.concatenate((cv_rgb, cv_depth), axis=1)

faces_message = self.bridge.cv2_to_imgmsg(rgbd, "bgr8")

self.pub.publish(faces_message)

Слева — RGB, справа — визуализированная глубина. Публикуется результат в отдельный топик.

Для запуска кейса необходимо остановить работу прошлого кода (сочетание клавиш CTRL+C) и ввести новую команду для начала работы(рис.28):

rosrun trackingcam3d_client_ros recognition_object.py

В случае ошибки работы matplotlib – закомментируйте строку в коде:
import matplotlib.pyplot as plt

Рис.28 Детектирование красного шарика и расчёт расстояния

По данным, которые приходят в терминал, можно заметить, что ошибка в сравнение с предыдущим кодом, намного меньше и расчёт дистанции схожий с реальностью. Это связано с использованием алгоритма Хафа для поиска окружностей, он основан на геометрии (поиск границ), а не на поиск цветовых масок. Однако, все еще необходимо учитывать освещение, яркость и тени.

Задача №2 Вычисление расстояния до объекта при неизвестных габаритах ​

Задача №2 Вычисление расстояния до объекта при неизвестных габаритах