Задача №3 Получение облака точек по RGBD данным
Облако точек — это набор данных в 3D-пространстве, где каждый объект или поверхность представлены множеством отдельных точек с координатами (X, Y, Z). Это основной формат для работы с 3D-данными в робототехнике, компьютерном зрении и дополненной реальности.
Разберем код детальнее, его можно найти по пути: /catkin_ws/src/applied_robotics-trackingcam3d_client_ros/src/convert_node.cpp
Основная его функция – преобразование карты глубины с модуля SVCAM в облако точек и публикует его в ROS.
- Подключение библиотек
#include <ros/ros.h\>
#include <cv_bridge/cv_bridge.h\> // для работы с изображениями OpenCV
#include <sensor_msgs/Image.h\> // для получения depth image
#include <sensor_msgs/CameraInfo.h\> // для параметров камеры
#include <sensor_msgs/PointCloud2.h\> // формат для 3D облака точек
#include <pcl_conversions/pcl_conversions.h\> // преобразование между
PCL и ROS
#include <pcl/point_types.h\>
#include <pcl/PCLPointCloud2.h\>
#include <pcl/conversions.h\>- Глобальные переменные
ros::Publisher pub_point_cloud2; // публикатор облака точек
bool is_K_empty = 1; // флаг, что параметры камеры ещё не получены
double K[9]; // матрица внутренней калибровки камеры- camera_info_callback()
void camera_info_callback(const sensor_msgs::CameraInfoConstPtr&camera_info_msg)Подписан на топик camera_info.
Извлекает матрицу калибровки K:
[ fx 0 cx ]
[ 0 fy cy ]
[ 0 0 1 ]Она нужна для восстановления 3D-координат из 2D.
- img_callback()
void img_callback(const sensor_msgs::ImageConstPtr &img_msg)Подписан на топик с картой глубины image_rect.
Шаг 1: чтение глубины
unsigned short *depth_data = (unsigned short*)&img_msg->data[0];глубина в формате 16UC1, единицы — миллиметры.
Шаг 2: преобразование в облако точек
z = depth / 1000.0; // перевод в метры
x = z * ((ux + width/2) - cx) / fx;
y = z * ((uy + height/2) - cy) / fy;Цикл по пикселям:
(ux, uy) — координаты пикселя.
z — глубина.
x, y — пересчитанные координаты по формуле идеальной камеры-обскуры.
Шаг 3: публикация
pcl::toROSMsg(*cloud, point_cloud2);
point_cloud2.header.frame_id = "stereo_camera_first";
pub_point_cloud2.publish(point_cloud2);Конвертация из PCL-облака в ROS-сообщение и публикация в /point_cloud.
- Функция main()
ros::Subscriber sub_img = n.subscribe(...);
ros::Subscriber sub_cmara_info = n.subscribe(...);
pub_point_cloud2 = n.advertise(...);Подписка на:
/image_rect — карта глубины;
/camera_info — параметры камеры.
Публикация в:
/point_cloud — готовое облако точек.
Для запуска кейса необходимо остановить предыдущий и запустить новый с помощью команды:rosrun trackingcam3d_client_ros convert_node
Чтобы увидеть данные необходимо в новом терминале прослушать топик «/point_cloud»
Для этого зайдем в Docker:
docker ps -a
docker exec –it <ID> /bin/bash
Далее требуется проинициализировать пространство:
cd
cd /catkin_ws
source devel/setup.bash
C помощью команды echо начнем прослушивать данные
rostopic echo /point_cloud
В терминале будут отображаться расстояние до каждого пикселя на изображении в байтах. В случае запуска данного кода на стационарном компьютере воспользуемся RViz для визуализации данных. В нем потребуется выставить параметры, как указано на рисунке 29.
Рис.29 Параметры в RViz для отображения облака точек
Также можно добавить видеопоток с камеры глубины и обычной камеры. В итоге при визуализации получим следующие результаты – рисунок 30.
Рис. 30 Облако точек совместно с видеопотоком камеры