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 c5b55a7a8..26e8e7908 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 @@ -136,6 +136,8 @@ class ROSImageTexture : public ROSImageTextureIface uint32_t width_; uint32_t height_; + uint32_t stride_; + std::shared_ptr> bufferptr_; // fields for float image running median computation bool normalize_; 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 713f64e1e..cd6c3be71 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 @@ -33,6 +33,7 @@ #include #include #include +#include #include #include #include @@ -222,11 +223,12 @@ bool ROSImageTexture::update() return false; } - ImageData image_data = setFormatAndNormalizeDataIfNecessary( - ImageData(image->encoding, image->data.data(), image->data.size())); - width_ = image->width; height_ = image->height; + stride_ = image->step; + + ImageData image_data = setFormatAndNormalizeDataIfNecessary( + ImageData(image->encoding, image->data.data(), image->data.size())); Ogre::Image ogre_image; try { @@ -250,6 +252,142 @@ bool ROSImageTexture::fillWithCurrentImage(sensor_msgs::msg::Image::ConstSharedP return new_image_; } +struct yuyv +{ + uint8_t y0; + uint8_t u; + uint8_t y1; + uint8_t v; +}; + +struct uyvy +{ + uint8_t u; + uint8_t y0; + uint8_t v; + uint8_t y1; +}; + +// Function converts src_img from yuv422 format to rgb +void imageConvertYUV422ToRGB( + uint8_t * dst_img, uint8_t * src_img, + int dst_start_row, int dst_end_row, + int dst_num_cols, uint32_t stride_in_bytes) +{ + int final_y0 = 0; + int final_u = 0; + int final_y1 = 0; + int final_v = 0; + int r1 = 0; + int b1 = 0; + int g1 = 0; + int r2 = 0; + int b2 = 0; + int g2 = 0; + + uint32_t stride_in_pixels = stride_in_bytes / 4; + + // rows in dst_img + for (int row = dst_start_row; row < dst_end_row; row++) { + // col iterates till num_cols / 2 since two rgb pixels processed each + // iteration cols in dst_img + for (int col = 0; col < dst_num_cols / 2; col++) { + struct uyvy * src_ptr = reinterpret_cast(src_img); + struct uyvy * pixel = &src_ptr[col + row * stride_in_pixels]; + final_y0 = pixel->y0; + final_u = pixel->u; + final_y1 = pixel->y1; + final_v = pixel->v; + + // Values generated based on this formula + // for converting YUV to RGB + // R = Y + 1.403V' + // G = Y + 0.344U' - 0.714V' + // B = Y + 1.770U' + + final_v -= 128; + final_u -= 128; + + r1 = final_y0 + (1403 * final_v) / 1000; + g1 = final_y0 + (344 * final_u - 714 * final_v) / 1000; + b1 = final_y0 + (1770 * final_u) / 1000; + + r2 = final_y1 + (1403 * final_v) / 1000; + g2 = final_y1 + (344 * final_u - 714 * final_v) / 1000; + b2 = final_y1 + (1770 * final_u) / 1000; + + // pixel value must fit in a uint8_t + dst_img[0] = ((r1 & 0xFFFFFF00) == 0) ? r1 : (r1 < 0) ? 0 : 0xFF; + dst_img[1] = ((g1 & 0xFFFFFF00) == 0) ? g1 : (g1 < 0) ? 0 : 0xFF; + dst_img[2] = ((b1 & 0xFFFFFF00) == 0) ? b1 : (b1 < 0) ? 0 : 0xFF; + dst_img[3] = ((r2 & 0xFFFFFF00) == 0) ? r2 : (r2 < 0) ? 0 : 0xFF; + dst_img[4] = ((g2 & 0xFFFFFF00) == 0) ? g2 : (g2 < 0) ? 0 : 0xFF; + dst_img[5] = ((b2 & 0xFFFFFF00) == 0) ? b2 : (b2 < 0) ? 0 : 0xFF; + dst_img += 6; + } + } +} + +// Function converts src_img from yuv422_yuy2 format to rgb +void imageConvertYUV422_YUY2ToRGB( + uint8_t * dst_img, uint8_t * src_img, + int dst_start_row, int dst_end_row, + int dst_num_cols, uint32_t stride_in_bytes) +{ + int final_y0 = 0; + int final_u = 0; + int final_y1 = 0; + int final_v = 0; + int r1 = 0; + int b1 = 0; + int g1 = 0; + int r2 = 0; + int b2 = 0; + int g2 = 0; + + uint32_t stride_in_pixels = stride_in_bytes / 4; + + // rows in dst_img + for (int row = dst_start_row; row < dst_end_row; row++) { + // col iterates till num_cols / 2 since two rgb pixels processed each + // iteration cols in dst_img + for (int col = 0; col < dst_num_cols / 2; col++) { + struct yuyv * src_ptr = reinterpret_cast(src_img); + struct yuyv * pixel = &src_ptr[col + row * stride_in_pixels]; + final_y0 = pixel->y0; + final_u = pixel->u; + final_y1 = pixel->y1; + final_v = pixel->v; + + // Values generated based on this formula + // for converting YUV to RGB + // R = Y + 1.403V' + // G = Y + 0.344U' - 0.714V' + // B = Y + 1.770U' + + final_v -= 128; + final_u -= 128; + + r1 = final_y0 + (1403 * final_v) / 1000; + g1 = final_y0 + (344 * final_u - 714 * final_v) / 1000; + b1 = final_y0 + (1770 * final_u) / 1000; + + r2 = final_y1 + (1403 * final_v) / 1000; + g2 = final_y1 + (344 * final_u - 714 * final_v) / 1000; + b2 = final_y1 + (1770 * final_u) / 1000; + + // pixel value must fit in a uint8_t + dst_img[0] = ((r1 & 0xFFFFFF00) == 0) ? r1 : (r1 < 0) ? 0 : 0xFF; + dst_img[1] = ((g1 & 0xFFFFFF00) == 0) ? g1 : (g1 < 0) ? 0 : 0xFF; + dst_img[2] = ((b1 & 0xFFFFFF00) == 0) ? b1 : (b1 < 0) ? 0 : 0xFF; + dst_img[3] = ((r2 & 0xFFFFFF00) == 0) ? r2 : (r2 < 0) ? 0 : 0xFF; + dst_img[4] = ((g2 & 0xFFFFFF00) == 0) ? g2 : (g2 < 0) ? 0 : 0xFF; + dst_img[5] = ((b2 & 0xFFFFFF00) == 0) ? b2 : (b2 < 0) ? 0 : 0xFF; + dst_img += 6; + } + } +} + ImageData ROSImageTexture::setFormatAndNormalizeDataIfNecessary(ImageData image_data) { if (image_data.encoding_ == sensor_msgs::image_encodings::RGB8) { @@ -294,6 +432,31 @@ ImageData ROSImageTexture::setFormatAndNormalizeDataIfNecessary(ImageData image_ image_data.size_); image_data.pixel_format_ = Ogre::PF_BYTE_L; image_data.data_ptr_ = &buffer[0]; + } else if ( // NOLINT enforces bracket on the same line, which makes code unreadable + image_data.encoding_ == sensor_msgs::image_encodings::YUV422 || + image_data.encoding_ == sensor_msgs::image_encodings::YUV422_YUY2) + { + size_t new_size = image_data.size_ * 3 / 2; + if (!bufferptr_) { + bufferptr_ = std::make_shared>(new_size); + } else if (static_cast(bufferptr_->size()) != new_size) { + bufferptr_->resize(new_size, 0); + } + + if (image_data.encoding_ == sensor_msgs::image_encodings::YUV422) { + imageConvertYUV422ToRGB( + bufferptr_->data(), const_cast(image_data.data_ptr_), + 0, height_, width_, stride_); + } else if (image_data.encoding_ == sensor_msgs::image_encodings::YUV422_YUY2) { + imageConvertYUV422_YUY2ToRGB( + bufferptr_->data(), const_cast(image_data.data_ptr_), + 0, height_, width_, stride_); + } + + + image_data.pixel_format_ = Ogre::PF_BYTE_RGB; + image_data.data_ptr_ = bufferptr_->data(); + image_data.size_ = new_size; } else { throw UnsupportedImageEncoding(image_data.encoding_); }