From 3349cdb10266ddbccc18ee889ef9fac9cf0b2eae Mon Sep 17 00:00:00 2001 From: zycczy Date: Thu, 26 Dec 2024 17:22:15 +0800 Subject: [PATCH 1/2] rviz: display: image: Add NV12 to RGB conversion support in rviz2 - Introduce a new `convertNV12ToRGBData` method for handling NV12 images - Correctly handle separate Y and UV planes within the NV12 buffer - Ensure proper stride usage for row and column indexing - Update `setFormatAndNormalizeDataIfNecessary` to recognize "nv12" encoding Signed-off-by: Zhaoyuan Cheng --- .../displays/image/ros_image_texture.hpp | 1 + .../displays/image/ros_image_texture.cpp | 44 +++++++++++++++++++ 2 files changed, 45 insertions(+) diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/image/ros_image_texture.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/image/ros_image_texture.hpp index c75ad960e..53ab17028 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/image/ros_image_texture.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/image/ros_image_texture.hpp @@ -130,6 +130,7 @@ class ROSImageTexture : public ROSImageTextureIface ImageData convertTo8bit(const uint8_t * data_ptr, size_t data_size_in_bytes); ImageData convertUYVYToRGBData(const uint8_t * data_ptr, size_t data_size_in_bytes); ImageData convertYUYVToRGBData(const uint8_t * data_ptr, size_t data_size_in_bytes); + ImageData convertNV12ToRGBData(const uint8_t * data_ptr, size_t data_size_in_bytes); ImageData setFormatAndNormalizeDataIfNecessary( const std::string & encoding, const uint8_t * data_ptr, size_t data_size_in_bytes); diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/image/ros_image_texture.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/image/ros_image_texture.cpp index 1626b08af..93b375bd5 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/image/ros_image_texture.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/image/ros_image_texture.cpp @@ -233,6 +233,34 @@ struct uyvy uint8_t y1; }; +// Function converts src_img from NV12 format to rgb +static void imageConvertNV12ToRGB( + uint8_t * dst_img, + const uint8_t * src_img, + int dst_start_row, + int dst_end_row, + int dst_num_cols, + uint32_t stride_in_bytes) +{ + const uint8_t * y_ptr = src_img; + const uint8_t * uv_ptr = src_img + (dst_end_row * stride_in_bytes); + + for (int row = dst_start_row; row < dst_end_row; row++) { + for (int col = 0; col < dst_num_cols; col++) { + int y_index = row * stride_in_bytes + col; + int uv_index = (row / 2) * stride_in_bytes + (col / 2) * 2; + + int final_y = y_ptr[y_index]; + int final_u = uv_ptr[uv_index + 0]; + int final_v = uv_ptr[uv_index + 1]; + + std::tie(dst_img[0], dst_img[1], dst_img[2]) = pixelYUVToRGB(final_y, final_u, final_v); + + dst_img += 3; + } + } +} + // Function converts src_img from UYVY format to rgb static void imageConvertUYVYToRGB( uint8_t * dst_img, uint8_t * src_img, @@ -409,6 +437,20 @@ ROSImageTexture::convertYUYVToRGBData(const uint8_t * data_ptr, size_t data_size return ImageData(Ogre::PF_BYTE_RGB, new_data, new_size_in_bytes, true); } +ImageData +ROSImageTexture::convertNV12ToRGBData(const uint8_t * data_ptr, size_t data_size_in_bytes) +{ + size_t new_size_in_bytes = data_size_in_bytes * 2; + + uint8_t * new_data = new uint8_t[new_size_in_bytes]; + + imageConvertNV12ToRGB( + new_data, const_cast(data_ptr), + 0, height_, width_, width_); + + return ImageData(Ogre::PF_BYTE_RGB, new_data, new_size_in_bytes, true); +} + ImageData ROSImageTexture::setFormatAndNormalizeDataIfNecessary( const std::string & encoding, const uint8_t * data_ptr, size_t data_size_in_bytes) @@ -449,6 +491,8 @@ ROSImageTexture::setFormatAndNormalizeDataIfNecessary( return convertUYVYToRGBData(data_ptr, data_size_in_bytes); } else if (encoding == sensor_msgs::image_encodings::YUYV) { return convertYUYVToRGBData(data_ptr, data_size_in_bytes); + } else if (encoding == sensor_msgs::image_encodings::NV12) { + return convertNV12ToRGBData(data_ptr, data_size_in_bytes); } else { throw UnsupportedImageEncoding(encoding); } From b406b3f52fd98b95144061c5bb68a83fac39684a Mon Sep 17 00:00:00 2001 From: quic-zhaoyuan <164289792+quic-zhaoyuan@users.noreply.github.com> Date: Fri, 10 Jan 2025 15:25:41 +0800 Subject: [PATCH 2/2] Update ros_image_texture.cpp update convertNV12ToRGBData, modify width_ to stride_ Signed-off-by: quic-zhaoyuan <164289792+quic-zhaoyuan@users.noreply.github.com> --- .../rviz_default_plugins/displays/image/ros_image_texture.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/image/ros_image_texture.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/image/ros_image_texture.cpp index 93b375bd5..347c0a9db 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/image/ros_image_texture.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/image/ros_image_texture.cpp @@ -446,7 +446,7 @@ ROSImageTexture::convertNV12ToRGBData(const uint8_t * data_ptr, size_t data_size imageConvertNV12ToRGB( new_data, const_cast(data_ptr), - 0, height_, width_, width_); + 0, height_, width_, stride_); return ImageData(Ogre::PF_BYTE_RGB, new_data, new_size_in_bytes, true); }