Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Yuv to rgb changes #701

Merged
merged 9 commits into from
Jul 14, 2021
Merged

Yuv to rgb changes #701

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
583f52e
added yuv to rgb Image conversion
cturcotte-qnx Jun 1, 2021
bd3c9e0
yuv2rgb fixes
cturcotte-qnx Jun 4, 2021
f3dc05d
optimized calculation
cturcotte-qnx Jun 4, 2021
322970f
reorder stride_
cturcotte-qnx Jun 4, 2021
8f43339
Merge pull request #1 from cturcotte-qnx/yuv_to_rgb
cturcotte-qnx Jun 7, 2021
2a7b3b6
formatting fixes
cturcotte-qnx Jun 8, 2021
952fcb8
formatting fixes
cturcotte-qnx Jun 8, 2021
941f229
formatting fixes
cturcotte-qnx Jun 8, 2021
9802313
requested changes
cturcotte-qnx Jun 9, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions rviz_default_plugins/include/rviz_default_plugins/displays/image/ros_image_texture.hpp
View file
Open in desktop
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
Open in desktop
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_);
}

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change


image_data.pixel_format_ = Ogre::PF_BYTE_RGB;
image_data.data_ptr_ = bufferptr_->data();
cturcotte-qnx marked this conversation as resolved.
Show resolved Hide resolved
image_data.size_ = new_size;
} else {
throw UnsupportedImageEncoding(image_data.encoding_);
}
Expand Down