Задача №4 Преобразование облака точек в формат данных лидара ​

Лидар отправляет данные в виде точек в 3D-пространстве или 2D-пространств, которые формируют облако точек. В нашем случае задача заключается в преобразовании точек 3D-пространства, получаемых с камеры, на плоскость. То есть имитация 2D лидара.

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

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

Разберем его детальнее:

  1. Импорт и инициализация
import roslib

import rospy

import cv2, numpy as np

import message_filters

from std_msgs.msg import String

from sensor_msgs.msg import Image, CameraInfo, LaserScan

from cv_bridge import CvBridge

Загружаются ROS-библиотеки, OpenCV и cv_bridge — мост между OpenCV и ROS.

  1. Класс get_face_distance_from_camera

В этом классе создаётся ROS-нода, подписывающаяся на 3 потока:

image_raw — цветное изображение (RGB),

depth/image_rect — карта глубины (float32 в метрах),

camera_info — параметры калибровки камеры.

self.ts = message_filters.ApproximateTimeSynchronizer(...)

self.ts.registerCallback(self.callback)

Эти три потока синхронизируются по времени с небольшой допустимой погрешностью (slop=0.5 сек), чтобы изображения и глубина совпадали.

  1. Публикация
self.scan_pub = rospy.Publisher('scan', LaserScan, queue_size=50)

Создаётся паблишер, который публикует LaserScan.

  1. Метод callback()

Выполняется каждый раз, когда приходят синхронизированные данные.

Чтение матрицы калибровки камеры:

camera_info.K = [fx, 0, cx, 0, fy, cy, 0, 0, 1]

Конвертация RGB и depth:

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

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

depth_image — карта глубины в метрах.

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

cv2.normalize(...)

Глубина преобразуется в ч/б изображение (cv_depth) для визуализации.

Отладочные точки:

print(roi_depth[40, 300]) # глубина в конкретных пикселях
  1. Основной цикл
for y in range(0, roi_depth.shape[0] - 25):

for x in range(0, self.num_readings):

Пробегает по всем пикселям (320 направлений) вдоль высоты изображения (y).

Переводит координаты пикселя и глубину в 3D-точки X, Y, Z в метрах, используя калибровку камеры.

point_x = ((x - cx) * Z) / fx

point_y = ((y - cy) * Z) / fy

5.Расчёт расстояния:

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

Но сохраняется только расстояние до ближайшей точки по каждому направлению.

6.Публикация результата как LaserScan:

self.scan_pub.publish(self.scan)

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

Такой подход позволяет использовать 3D-камеру как 2D-лидар — например, для локализации, SLAM, навигации.

Для отображения данных RViz необходимо поставить галочку в «PointCloud» «LaserScan»(рис.31). Таким образом видно преобразование облака точек на плоскости.

Рис.31 Преобразование облака точек на плоскость

Также в новом терминале можно прописать команды для получения данных из топиков

rostopic echo /scan

rostopic echo /point_cloud