Задача №5 Распознавание лица и определение расстояния

В данном коде реализуется задача по получению изображения и карты глубины с модуля SVCAM. После происходит обработка и обнаружения лица человека, при положительном исходе, в командную строку отправляется расстояние.

Код программы можно найти:

/catkin_ws/src/applied_robotics-trackingcam3d_client_ros/src/face_recognition.py

Разберем код программы:

Подключение библиотек

import rospy, cv2, numpy as np, message_filters, math

from sensor_msgs.msg import Image, CameraInfo

from cv_bridge import CvBridge

Загружаются ROS, OpenCV, фильтры сообщений и функции для конвертации сообщений в изображения.

Класс get_face_distance_from_camera

Подписка на данные

self.image_sub = Subscriber("/.../image_raw", Image)

self.depth_sub = Subscriber("/.../depth/image_rect", Image)

self.camera_info_sub = Subscriber("/.../camera_info", CameraInfo)

Получает цветное изображение, глубину и матрицу камеры.

self.ts = ApproximateTimeSynchronizer([...])

self.ts.registerCallback(self.callback)

Синхронизирует 3 потока по времени.

callback: Главная логика

Выполняется при поступлении новых синхронных RGB, Depth и CameraInfo сообщений.

fx = camera_info.K[0], fy = K[4], cx = K[2], cy = K[5]

Чтение матрицы камеры, используется для пересчёта координат пикселя в метры.

Конвертация и подготовка изображений

cv_rgb = imgmsg_to_cv2(rgb_data, "bgr8")

depth_image = imgmsg_to_cv2(depth_data, "32FC1")

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

Цветное и глубинное изображения из ROS в OpenCV.

Глубина масштабируется в 640×480 (если необходимо).

Нормализация глубины (для визуализации)

depth_array → normalize → depth_8 → cv_depth

Создаётся 3-канальное серое изображение глубины для отображения.

Обнаружение лиц (Haar каскад)

face_cascade = cv2.CascadeClassifier('haar_cascade.xml')

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

faces = face_cascade.detectMultiScale(gray, 1.3, 5)

Ищет прямоугольники лиц на ч/б изображении.

Расчёт расстояния до лица

Для каждого найденного лица:

roi_depth = depth_image_re[y+30:y+h-30, x+30:x+w-30]

Выбирается внутренний регион интереса (ROI) в глубинном изображении.

n = 0; sum = 0

for i, j in ROI: if value > 0 → sum += value

mean_z = sum / n

Вычисляется средняя глубина внутри прямоугольника.

Потом — пересчёт в координаты X, Y, Z:

point_z = mean_z * 0.001 # метры

point_x = ((x + w/2) - cx) * point_z / fx

point_y = ((y + h/2) - cy) * point_z / fy

Расстояние:

dist = sqrt(x² + y² + z²)

Отображение на изображении

cv2.rectangle(...) # обводит лицо

cv2.putText(cv_rgb, dist_str, ...) # пишет расстояние до него

Публикация результата

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

msg = cv2_to_imgmsg(rgbd, "bgr8")

self.pub.publish(msg)

Склеивает цветное и глубинное изображение.

Публикует результат в /trackingcam3d_client_ros/faces_recognition.

Для запуска кейса необходимо остановить предыдущий и запустить новый с помощью команды:
rosrun trackingcam3d_client_ros face_recognition.py

В терминал будут приходить данные расстояния до распознанного лица.

При использовании ПК в RViz можно получить изображение (рис.32).

Рис.32 Расстояние до распознанного лица человека

Задача №5 Распознавание лица и определение расстояния ​

Задача №5 Распознавание лица и определение расстояния