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 26e8e7908..ad91dd6a1 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 @@ -60,14 +60,25 @@ class UnsupportedImageEncoding : public std::runtime_error {} }; -struct ImageData +struct ImageData final { - ImageData(std::string encoding, const uint8_t * data_ptr, size_t size); + ImageData( + Ogre::PixelFormat pixformat, + const uint8_t * data_ptr, + size_t data_size_in_bytes, + bool take_ownership); + + ~ImageData(); - std::string encoding_; Ogre::PixelFormat pixel_format_; const uint8_t * data_ptr_; - size_t size_; + size_t size_in_bytes_; + // Depending on the input format of the data from the ROS message, we may or may not need to do + // some kind of conversion. In the case where we do *not* do a conversion, we directly use the + // data pointer from the sensor_msgs::msg::Image::ConstSharedPtr and don't do any allocations for + // performance reasons. In the case where we *do* a conversion, we allocate memory and then this + // class will take ownership of the data and will be responsible for freeing it. + bool has_ownership_; }; class ROSImageTexture : public ROSImageTextureIface @@ -86,19 +97,19 @@ class ROSImageTexture : public ROSImageTextureIface void clear() override; RVIZ_DEFAULT_PLUGINS_PUBLIC - const Ogre::String getName() override {return texture_->getName();} + const Ogre::String getName() const override; RVIZ_DEFAULT_PLUGINS_PUBLIC - const Ogre::TexturePtr & getTexture() override {return texture_;} + const Ogre::TexturePtr & getTexture() override; RVIZ_DEFAULT_PLUGINS_PUBLIC const sensor_msgs::msg::Image::ConstSharedPtr getImage() override; RVIZ_DEFAULT_PLUGINS_PUBLIC - uint32_t getWidth() override {return width_;} + uint32_t getWidth() const override; RVIZ_DEFAULT_PLUGINS_PUBLIC - uint32_t getHeight() override {return height_;} + uint32_t getHeight() const override; // automatic range normalization RVIZ_DEFAULT_PLUGINS_PUBLIC @@ -111,21 +122,16 @@ class ROSImageTexture : public ROSImageTextureIface void setMedianFrames(unsigned median_frames) override; private: - template - std::vector normalize(const T * image_data, size_t image_data_size); - template - std::vector createNewNormalizedBuffer( - const T * image_data, size_t image_data_size, T minValue, T maxValue) const; - double computeMedianOfSeveralFrames(std::deque & buffer, double new_value); - void updateBuffer(std::deque & buffer, double value) const; - double computeMedianOfBuffer(const std::deque & buffer) const; template void getMinimalAndMaximalValueToNormalize( - const T * image_data, size_t image_data_size, T & minValue, T & maxValue); + const T * data_ptr, size_t num_elements, T & min_value, T & max_value); + template + ImageData convertTo8bit(const uint8_t * data_ptr, size_t data_size_in_bytes); + ImageData convertYUV422ToRGBData(const uint8_t * data_ptr, size_t data_size_in_bytes); + ImageData convertYUV422_YUY2ToRGBData(const uint8_t * data_ptr, size_t data_size_in_bytes); - bool fillWithCurrentImage(sensor_msgs::msg::Image::ConstSharedPtr & image); - ImageData setFormatAndNormalizeDataIfNecessary(ImageData image_data); - void loadImageToOgreImage(const ImageData & image_data, Ogre::Image & ogre_image) const; + ImageData setFormatAndNormalizeDataIfNecessary( + const std::string & encoding, const uint8_t * data_ptr, size_t data_size_in_bytes); sensor_msgs::msg::Image::ConstSharedPtr current_image_; std::mutex mutex_; @@ -137,7 +143,6 @@ 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/include/rviz_default_plugins/displays/image/ros_image_texture_iface.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/image/ros_image_texture_iface.hpp index ebc71ecf1..b46d4ec4b 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/image/ros_image_texture_iface.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/image/ros_image_texture_iface.hpp @@ -51,12 +51,12 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC ROSImageTextureIface virtual bool update() = 0; virtual void clear() = 0; - virtual const Ogre::String getName() = 0; + virtual const Ogre::String getName() const = 0; virtual const Ogre::TexturePtr & getTexture() = 0; virtual const sensor_msgs::msg::Image::ConstSharedPtr getImage() = 0; - virtual uint32_t getWidth() = 0; - virtual uint32_t getHeight() = 0; + virtual uint32_t getWidth() const = 0; + virtual uint32_t getHeight() const = 0; // automatic range normalization virtual void setNormalizeFloatImage(bool normalize) = 0; 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 cd6c3be71..e3c0ac413 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 @@ -44,6 +44,7 @@ #include // NOLINT: cpplint cannot handle include order #include "sensor_msgs/image_encodings.hpp" +#include "sensor_msgs/msg/image.hpp" #include "rviz_common/logging.hpp" #include "rviz_common/uniform_string_stream.hpp" @@ -88,6 +89,16 @@ void ROSImageTexture::clear() current_image_.reset(); } +const Ogre::String ROSImageTexture::getName() const +{ + return texture_->getName(); +} + +const Ogre::TexturePtr & ROSImageTexture::getTexture() +{ + return texture_; +} + const sensor_msgs::msg::Image::ConstSharedPtr ROSImageTexture::getImage() { std::lock_guard lock(mutex_); @@ -95,6 +106,16 @@ const sensor_msgs::msg::Image::ConstSharedPtr ROSImageTexture::getImage() return current_image_; } +uint32_t ROSImageTexture::getWidth() const +{ + return width_; +} + +uint32_t ROSImageTexture::getHeight() const +{ + return height_; +} + void ROSImageTexture::setMedianFrames(unsigned median_frames) { median_frames_ = median_frames; @@ -112,108 +133,27 @@ void ROSImageTexture::setNormalizeFloatImage(bool normalize, double min, double max_ = max; } -template -std::vector -ROSImageTexture::normalize(const T * image_data, size_t image_data_size) -{ - T minValue; - T maxValue; - - getMinimalAndMaximalValueToNormalize(image_data, image_data_size, minValue, maxValue); - - return createNewNormalizedBuffer(image_data, image_data_size, minValue, maxValue); -} - -template -void -ROSImageTexture::getMinimalAndMaximalValueToNormalize( - const T * image_data, size_t image_data_size, - T & minValue, T & maxValue) -{ - if (normalize_) { - const T * input_ptr = image_data; - - minValue = std::numeric_limits::max(); - maxValue = std::numeric_limits::min(); - for (unsigned int i = 0; i < image_data_size; ++i) { - minValue = std::min(minValue, *input_ptr); - maxValue = std::max(maxValue, *input_ptr); - input_ptr++; - } - - if (median_frames_ > 1) { - minValue = static_cast(computeMedianOfSeveralFrames(min_buffer_, minValue)); - maxValue = static_cast(computeMedianOfSeveralFrames(max_buffer_, maxValue)); - } - } else { - minValue = static_cast(min_); - maxValue = static_cast(max_); - } -} - -double ROSImageTexture::computeMedianOfSeveralFrames(std::deque & buffer, double value) -{ - updateBuffer(buffer, value); - return computeMedianOfBuffer(buffer); -} - -void ROSImageTexture::updateBuffer(std::deque & buffer, double value) const +static double +computeMedianOfSeveralFrames(std::deque & buffer, double value, unsigned median_frames) { - while (buffer.size() > median_frames_ - 1) { + while (buffer.size() > median_frames - 1) { buffer.pop_back(); } buffer.push_front(value); -} -double ROSImageTexture::computeMedianOfBuffer(const std::deque & buffer) const -{ + // Compute the median std::deque buffer2 = buffer; nth_element(buffer2.begin(), buffer2.begin() + buffer2.size() / 2, buffer2.end()); return *(buffer2.begin() + buffer2.size() / 2); } -template -std::vector -ROSImageTexture::createNewNormalizedBuffer( - const T * image_data, - size_t image_data_size, - T minValue, - T maxValue) const -{ - std::vector buffer; - buffer.resize(image_data_size, 0); - uint8_t * output_ptr = &buffer[0]; - - // Rescale floating point image and convert it to 8-bit - double range = maxValue - minValue; - if (range > 0.0) { - const T * input_ptr = image_data; - - // Rescale and quantize - for (size_t i = 0; i < image_data_size; ++i, ++output_ptr, ++input_ptr) { - double val = (static_cast(*input_ptr - minValue) / range); - if (val < 0) {val = 0;} - if (val > 1) {val = 1;} - *output_ptr = static_cast(val * 255u); - } - } - return buffer; -} - -ImageData::ImageData(std::string encoding, const uint8_t * data_ptr, size_t size) -: encoding_(std::move(encoding)), - pixel_format_(Ogre::PixelFormat::PF_R8G8B8), - data_ptr_(data_ptr), - size_(size) -{ -} - bool ROSImageTexture::update() { - sensor_msgs::msg::Image::ConstSharedPtr image; - bool has_new_image = fillWithCurrentImage(image); + std::lock_guard lock(mutex_); + + sensor_msgs::msg::Image::ConstSharedPtr image = current_image_; - if (!image || !has_new_image) { + if (!image || !new_image_) { return false; } @@ -228,12 +168,17 @@ bool ROSImageTexture::update() stride_ = image->step; ImageData image_data = setFormatAndNormalizeDataIfNecessary( - ImageData(image->encoding, image->data.data(), image->data.size())); + image->encoding, image->data.data(), image->data.size()); Ogre::Image ogre_image; try { - loadImageToOgreImage(image_data, ogre_image); - } catch (Ogre::Exception & e) { + Ogre::DataStreamPtr pixel_stream; + pixel_stream.reset( + new Ogre::MemoryDataStream( + const_cast(&image_data.data_ptr_[0]), + image_data.size_in_bytes_)); + ogre_image.loadRawData(pixel_stream, width_, height_, 1, image_data.pixel_format_, 1, 0); + } catch (const Ogre::Exception & e) { RVIZ_COMMON_LOG_ERROR_STREAM("Error loading image: " << e.what()); return false; } @@ -244,14 +189,6 @@ bool ROSImageTexture::update() return true; } -bool ROSImageTexture::fillWithCurrentImage(sensor_msgs::msg::Image::ConstSharedPtr & image) -{ - std::lock_guard lock(mutex_); - - image = current_image_; - return new_image_; -} - struct yuyv { uint8_t y0; @@ -269,7 +206,7 @@ struct uyvy }; // Function converts src_img from yuv422 format to rgb -void imageConvertYUV422ToRGB( +static 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) @@ -329,7 +266,7 @@ void imageConvertYUV422ToRGB( } // Function converts src_img from yuv422_yuy2 format to rgb -void imageConvertYUV422_YUY2ToRGB( +static 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) @@ -388,91 +325,161 @@ void imageConvertYUV422_YUY2ToRGB( } } -ImageData ROSImageTexture::setFormatAndNormalizeDataIfNecessary(ImageData image_data) +ImageData::ImageData( + Ogre::PixelFormat pixformat, + const uint8_t * data_ptr, + size_t data_size_in_bytes, + bool take_ownership) +: pixel_format_(pixformat), + data_ptr_(data_ptr), + size_in_bytes_(data_size_in_bytes), + has_ownership_(take_ownership) +{ +} + +ImageData::~ImageData() +{ + if (has_ownership_) { + delete[] data_ptr_; + } +} + +template +void +ROSImageTexture::getMinimalAndMaximalValueToNormalize( + const T * data_ptr, size_t num_elements, + T & min_value, T & max_value) +{ + if (normalize_) { + const T * input_ptr = data_ptr; + + min_value = std::numeric_limits::max(); + max_value = std::numeric_limits::min(); + for (size_t i = 0; i < num_elements; ++i) { + min_value = std::min(min_value, *input_ptr); + max_value = std::max(max_value, *input_ptr); + input_ptr++; + } + + if (median_frames_ > 1) { + min_value = + static_cast( + computeMedianOfSeveralFrames(min_buffer_, min_value, this->median_frames_)); + max_value = + static_cast( + computeMedianOfSeveralFrames(max_buffer_, max_value, this->median_frames_)); + } + } else { + min_value = static_cast(min_); + max_value = static_cast(max_); + } +} + +template +ImageData +ROSImageTexture::convertTo8bit(const uint8_t * data_ptr, size_t data_size_in_bytes) +{ + size_t new_size_in_bytes = data_size_in_bytes / sizeof(T); + + uint8_t * new_data = new uint8_t[new_size_in_bytes]; + + T min_value; + T max_value; + + getMinimalAndMaximalValueToNormalize( + reinterpret_cast(data_ptr), new_size_in_bytes, min_value, max_value); + + uint8_t * output_ptr = new_data; + + // Rescale T image and convert it to 8-bit + double range = max_value - min_value; + if (range > 0.0) { + const T * input_ptr = reinterpret_cast(data_ptr); + + // Rescale and quantize + for (size_t i = 0; i < new_size_in_bytes; ++i, ++output_ptr, ++input_ptr) { + double val = (static_cast(*input_ptr - min_value) / range); + if (val < 0) {val = 0;} + if (val > 1) {val = 1;} + *output_ptr = static_cast(val * 255u); + } + } + // TODO(clalancette): What happens when range is <= 0.0? + + return ImageData(Ogre::PF_BYTE_L, new_data, new_size_in_bytes, true); +} + +ImageData +ROSImageTexture::convertYUV422ToRGBData(const uint8_t * data_ptr, size_t data_size_in_bytes) +{ + size_t new_size_in_bytes = data_size_in_bytes * 3 / 2; + + uint8_t * new_data = new uint8_t[new_size_in_bytes]; + + imageConvertYUV422ToRGB( + new_data, const_cast(data_ptr), + 0, height_, width_, stride_); + + return ImageData(Ogre::PF_BYTE_RGB, new_data, new_size_in_bytes, true); +} + +ImageData +ROSImageTexture::convertYUV422_YUY2ToRGBData(const uint8_t * data_ptr, size_t data_size_in_bytes) { - if (image_data.encoding_ == sensor_msgs::image_encodings::RGB8) { - image_data.pixel_format_ = Ogre::PF_BYTE_RGB; - } else if (image_data.encoding_ == sensor_msgs::image_encodings::RGBA8) { - image_data.pixel_format_ = Ogre::PF_BYTE_RGBA; + size_t new_size_in_bytes = data_size_in_bytes * 3 / 2; + + uint8_t * new_data = new uint8_t[new_size_in_bytes]; + + imageConvertYUV422_YUY2ToRGB( + new_data, const_cast(data_ptr), + 0, height_, width_, stride_); + + 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) +{ + if (encoding == sensor_msgs::image_encodings::RGB8) { + return ImageData(Ogre::PF_BYTE_RGB, data_ptr, data_size_in_bytes, false); + } else if (encoding == sensor_msgs::image_encodings::RGBA8) { + return ImageData(Ogre::PF_BYTE_RGBA, data_ptr, data_size_in_bytes, false); } else if ( // NOLINT enforces bracket on the same line, which makes code unreadable - image_data.encoding_ == sensor_msgs::image_encodings::TYPE_8UC4 || - image_data.encoding_ == sensor_msgs::image_encodings::TYPE_8SC4 || - image_data.encoding_ == sensor_msgs::image_encodings::BGRA8) + encoding == sensor_msgs::image_encodings::TYPE_8UC4 || + encoding == sensor_msgs::image_encodings::TYPE_8SC4 || + encoding == sensor_msgs::image_encodings::BGRA8) { - image_data.pixel_format_ = Ogre::PF_BYTE_BGRA; + return ImageData(Ogre::PF_BYTE_BGRA, data_ptr, data_size_in_bytes, false); } else if ( // NOLINT enforces bracket on the same line, which makes code unreadable - image_data.encoding_ == sensor_msgs::image_encodings::TYPE_8UC3 || - image_data.encoding_ == sensor_msgs::image_encodings::TYPE_8SC3 || - image_data.encoding_ == sensor_msgs::image_encodings::BGR8) + encoding == sensor_msgs::image_encodings::TYPE_8UC3 || + encoding == sensor_msgs::image_encodings::TYPE_8SC3 || + encoding == sensor_msgs::image_encodings::BGR8) { - image_data.pixel_format_ = Ogre::PF_BYTE_BGR; + return ImageData(Ogre::PF_BYTE_BGR, data_ptr, data_size_in_bytes, false); } else if ( // NOLINT enforces bracket on the same line, which makes code unreadable - image_data.encoding_ == sensor_msgs::image_encodings::TYPE_8UC1 || - image_data.encoding_ == sensor_msgs::image_encodings::TYPE_8SC1 || - image_data.encoding_ == sensor_msgs::image_encodings::MONO8) + encoding == sensor_msgs::image_encodings::TYPE_8UC1 || + encoding == sensor_msgs::image_encodings::TYPE_8SC1 || + encoding == sensor_msgs::image_encodings::MONO8) { - image_data.pixel_format_ = Ogre::PF_BYTE_L; + return ImageData(Ogre::PF_BYTE_L, data_ptr, data_size_in_bytes, false); } else if ( // NOLINT enforces bracket on the same line, which makes code unreadable - image_data.encoding_ == sensor_msgs::image_encodings::TYPE_16UC1 || - image_data.encoding_ == sensor_msgs::image_encodings::TYPE_16SC1 || - image_data.encoding_ == sensor_msgs::image_encodings::MONO16) + encoding == sensor_msgs::image_encodings::TYPE_16UC1 || + encoding == sensor_msgs::image_encodings::TYPE_16SC1 || + encoding == sensor_msgs::image_encodings::MONO16) { - image_data.size_ /= sizeof(uint16_t); - std::vector buffer = normalize( - reinterpret_cast(image_data.data_ptr_), - image_data.size_); - image_data.pixel_format_ = Ogre::PF_BYTE_L; - image_data.data_ptr_ = &buffer[0]; - } else if (image_data.encoding_.find("bayer") == 0) { - image_data.pixel_format_ = Ogre::PF_BYTE_L; - } else if (image_data.encoding_ == sensor_msgs::image_encodings::TYPE_32FC1) { - image_data.size_ /= sizeof(float); - std::vector buffer = normalize( - reinterpret_cast(image_data.data_ptr_), - 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; + return convertTo8bit(data_ptr, data_size_in_bytes); + } else if (encoding.find("bayer") == 0) { + return ImageData(Ogre::PF_BYTE_L, data_ptr, data_size_in_bytes, false); + } else if (encoding == sensor_msgs::image_encodings::TYPE_32FC1) { + return convertTo8bit(data_ptr, data_size_in_bytes); + } else if (encoding == sensor_msgs::image_encodings::YUV422) { + return convertYUV422ToRGBData(data_ptr, data_size_in_bytes); + } else if (encoding == sensor_msgs::image_encodings::YUV422_YUY2) { + return convertYUV422_YUY2ToRGBData(data_ptr, data_size_in_bytes); } else { - throw UnsupportedImageEncoding(image_data.encoding_); + throw UnsupportedImageEncoding(encoding); } - return image_data; -} - -void ROSImageTexture::loadImageToOgreImage( - const ImageData & image_data, - Ogre::Image & ogre_image) const -{ - Ogre::DataStreamPtr pixel_stream; - // C-style cast is used to bypass the const modifier - pixel_stream.reset( - new Ogre::MemoryDataStream( - (uint8_t *) &image_data.data_ptr_[0], image_data.size_)); // NOLINT - ogre_image.loadRawData(pixel_stream, width_, height_, 1, image_data.pixel_format_, 1, 0); } void ROSImageTexture::addMessage(sensor_msgs::msg::Image::ConstSharedPtr msg) diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/image/mock_ros_image_texture.hpp b/rviz_default_plugins/test/rviz_default_plugins/displays/image/mock_ros_image_texture.hpp index 8a9478762..d2dd3796c 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/image/mock_ros_image_texture.hpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/image/mock_ros_image_texture.hpp @@ -32,6 +32,8 @@ #include +#include "sensor_msgs/msg/image.hpp" + #include "rviz_default_plugins/displays/image/ros_image_texture_iface.hpp" class MockROSImageTexture : public rviz_default_plugins::displays::ROSImageTextureIface @@ -41,13 +43,13 @@ class MockROSImageTexture : public rviz_default_plugins::displays::ROSImageTextu MOCK_METHOD0(update, bool()); MOCK_METHOD0(clear, void()); - MOCK_METHOD0(getName, const Ogre::String()); + MOCK_METHOD(const Ogre::String, getName, (), (const, override)); MOCK_METHOD0(getTexture, const Ogre::TexturePtr & ()); MOCK_METHOD0(getImage, const sensor_msgs::msg::Image::ConstSharedPtr()); MOCK_METHOD0(getMaterial, Ogre::MaterialPtr()); - MOCK_METHOD0(getWidth, uint32_t()); - MOCK_METHOD0(getHeight, uint32_t()); + MOCK_METHOD(uint32_t, getWidth, (), (const, override)); + MOCK_METHOD(uint32_t, getHeight, (), (const, override)); MOCK_METHOD1(setNormalizeFloatImage, void(bool normalize)); MOCK_METHOD3(setNormalizeFloatImage, void(bool normalize, double min, double max));