Skip to content

Commit

Permalink
Yuv to rgb changes (#701)
Browse files Browse the repository at this point in the history 
* added yuv to rgb Image conversion

* yuv2rgb fixes

* optimized calculation

* reorder stride_

* formatting fixes

* formatting fixes

* formatting fixes

* requested changes
  • Loading branch information
@cturcotte-qnx
cturcotte-qnx authored Jul 14, 2021
1 parent 2d84e19 commit add6b0a
Show file tree
Hide file tree
Showing 2 changed files with 168 additions and 3 deletions.
2 changes: 2 additions & 0 deletions rviz_default_plugins/include/rviz_default_plugins/displays/image/ros_image_texture.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -136,6 +136,8 @@ class ROSImageTexture : public ROSImageTextureIface

uint32_t width_;
uint32_t height_;
uint32_t stride_;
std::shared_ptr<std::vector<uint8_t>> bufferptr_;

// fields for float image running median computation
bool normalize_;
Expand Down
169 changes: 166 additions & 3 deletions rviz_default_plugins/src/rviz_default_plugins/displays/image/ros_image_texture.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@
#include <deque>
#include <limits>
#include <map>
#include <memory>
#include <mutex>
#include <sstream>
#include <iostream>
Expand Down Expand Up @@ -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 {
Expand All @@ -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<struct uyvy *>(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<struct yuyv *>(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) {
Expand Down Expand Up @@ -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<std::vector<uint8_t>>(new_size);
} else if (static_cast<size_t>(bufferptr_->size()) != new_size) {
bufferptr_->resize(new_size, 0);
}

if (image_data.encoding_ == sensor_msgs::image_encodings::YUV422) {
imageConvertYUV422ToRGB(
bufferptr_->data(), const_cast<uint8_t *>(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<uint8_t *>(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_);
}
Expand Down

0 comments on commit add6b0a

Please sign in to comment.