From 75638cbd206a27cd945665c68a5962060f66056d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 19 Jan 2024 16:05:31 +0100 Subject: [PATCH] support rgba8 and bgra8 encodings by skipping alpha channel (#869) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Related with the change in ROS 1 https://github.com/ros-perception/image_pipeline/pull/671/files --------- Signed-off-by: Alejandro Hernández Cordero --- depth_image_proc/src/point_cloud_xyzrgb.cpp | 10 +++++ .../src/point_cloud_xyzrgb_radial.cpp | 10 +++++ .../stereo_image_proc/stereo_processor.cpp | 44 +++++++++++++++++++ 3 files changed, 64 insertions(+) diff --git a/depth_image_proc/src/point_cloud_xyzrgb.cpp b/depth_image_proc/src/point_cloud_xyzrgb.cpp index af33b9ce3..f5d3c0380 100644 --- a/depth_image_proc/src/point_cloud_xyzrgb.cpp +++ b/depth_image_proc/src/point_cloud_xyzrgb.cpp @@ -207,11 +207,21 @@ void PointCloudXyzrgbNode::imageCb( green_offset = 1; blue_offset = 2; color_step = 3; + } else if (rgb_msg->encoding == sensor_msgs::image_encodings::RGBA8) { + red_offset = 0; + green_offset = 1; + blue_offset = 2; + color_step = 4; } else if (rgb_msg->encoding == sensor_msgs::image_encodings::BGR8) { red_offset = 2; green_offset = 1; blue_offset = 0; color_step = 3; + } else if (rgb_msg->encoding == sensor_msgs::image_encodings::BGRA8) { + red_offset = 2; + green_offset = 1; + blue_offset = 0; + color_step = 4; } else if (rgb_msg->encoding == sensor_msgs::image_encodings::MONO8) { red_offset = 0; green_offset = 0; diff --git a/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp b/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp index 808a9b3fb..55e0e23d3 100644 --- a/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp +++ b/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp @@ -205,11 +205,21 @@ void PointCloudXyzrgbRadialNode::imageCb( green_offset = 1; blue_offset = 2; color_step = 3; + } else if (rgb_msg->encoding == sensor_msgs::image_encodings::RGBA8) { + red_offset = 0; + green_offset = 1; + blue_offset = 2; + color_step = 4; } else if (rgb_msg->encoding == sensor_msgs::image_encodings::BGR8) { red_offset = 2; green_offset = 1; blue_offset = 0; color_step = 3; + } else if (rgb_msg->encoding == sensor_msgs::image_encodings::BGRA8) { + red_offset = 2; + green_offset = 1; + blue_offset = 0; + color_step = 4; } else if (rgb_msg->encoding == sensor_msgs::image_encodings::MONO8) { red_offset = 0; green_offset = 0; diff --git a/stereo_image_proc/src/stereo_image_proc/stereo_processor.cpp b/stereo_image_proc/src/stereo_image_proc/stereo_processor.cpp index 093419d3c..1c02d96a7 100644 --- a/stereo_image_proc/src/stereo_image_proc/stereo_processor.cpp +++ b/stereo_image_proc/src/stereo_image_proc/stereo_processor.cpp @@ -214,6 +214,26 @@ void StereoProcessor::processPoints( } } } + } else if (encoding == enc::RGBA8) { + for (int32_t u = 0; u < dense_points_.rows; ++u) { + for (int32_t v = 0; v < dense_points_.cols; ++v) { + if (isValidPoint(dense_points_(u, v))) { + const cv::Vec4b & rgb = color.at(u, v); + int32_t rgb_packed = (rgb[0] << 16) | (rgb[1] << 8) | rgb[2]; + points.channels[0].values.push_back(*reinterpret_cast(&rgb_packed)); + } + } + } + } else if (encoding == enc::BGRA8) { + for (int32_t u = 0; u < dense_points_.rows; ++u) { + for (int32_t v = 0; v < dense_points_.cols; ++v) { + if (isValidPoint(dense_points_(u, v))) { + const cv::Vec4b & bgr = color.at(u, v); + int32_t rgb_packed = (bgr[2] << 16) | (bgr[1] << 8) | bgr[0]; + points.channels[0].values.push_back(*reinterpret_cast(&rgb_packed)); + } + } + } } else if (encoding == enc::BGR8) { for (int32_t u = 0; u < dense_points_.rows; ++u) { for (int32_t v = 0; v < dense_points_.cols; ++v) { @@ -317,6 +337,18 @@ void StereoProcessor::processPoints2( } } } + } else if (encoding == enc::RGBA8) { + for (int32_t u = 0; u < dense_points_.rows; ++u) { + for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) { + if (isValidPoint(dense_points_(u, v))) { + const cv::Vec4b & rgb = color.at(u, v); + int32_t rgb_packed = (rgb[0] << 16) | (rgb[1] << 8) | rgb[2]; + memcpy(&points.data[i * points.point_step + 12], &rgb_packed, sizeof(int32_t)); + } else { + memcpy(&points.data[i * points.point_step + 12], &bad_point, sizeof(float)); + } + } + } } else if (encoding == enc::BGR8) { for (int32_t u = 0; u < dense_points_.rows; ++u) { for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) { @@ -329,6 +361,18 @@ void StereoProcessor::processPoints2( } } } + } else if (encoding == enc::BGRA8) { + for (int32_t u = 0; u < dense_points_.rows; ++u) { + for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) { + if (isValidPoint(dense_points_(u, v))) { + const cv::Vec4b & bgr = color.at(u, v); + int32_t rgb_packed = (bgr[2] << 16) | (bgr[1] << 8) | bgr[0]; + memcpy(&points.data[i * points.point_step + 12], &rgb_packed, sizeof(int32_t)); + } else { + memcpy(&points.data[i * points.point_step + 12], &bad_point, sizeof(float)); + } + } + } } else { RCUTILS_LOG_WARN( "Could not fill color channel of the point cloud, unrecognized encoding '%s'",