diff --git a/docs/FEATURES.md b/docs/FEATURES.md index ef6204be9..1291de688 100644 --- a/docs/FEATURES.md +++ b/docs/FEATURES.md @@ -11,6 +11,11 @@ In addition and as before, it needs information about the camera via a `sensor_m *Changed behaviour:* The CameraInfo topic is assumed to be relative to the Image topic: If the image topic is "/image", the CameraInfo topic is assumed to be "/image/camera_info". +## Image Display + +Bayer-encoded images (the eight `bayer_*` encodings, 8- and 16-bit) are demosaiced and shown in color, in both the Image and Camera displays. +The *Linear input* property applies sRGB encoding for cameras that publish linear-response Bayer images. + ## Map Display #### Topics diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/image/bayer_format.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/image/bayer_format.hpp new file mode 100644 index 000000000..201258813 --- /dev/null +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/image/bayer_format.hpp @@ -0,0 +1,111 @@ +// Copyright (c) 2026, Arne Baeyens. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + + +#ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__IMAGE__BAYER_FORMAT_HPP_ +#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__IMAGE__BAYER_FORMAT_HPP_ + +#include +#include + +#include "sensor_msgs/image_encodings.hpp" + +namespace rviz_default_plugins +{ +namespace displays +{ + +// The position of R within the 2x2 Bayer cell uniquely identifies the layout. +// B sits at the opposite corner; G occupies the other two cells. +// RGGB -> R at (0, 0) +// BGGR -> R at (1, 1) +// GBRG -> R at (1, 0) +// GRBG -> R at (0, 1) +enum class BayerLayout +{ + RGGB, + BGGR, + GBRG, + GRBG +}; + +constexpr int bayerRedRow(BayerLayout layout) +{ + return (layout == BayerLayout::BGGR || layout == BayerLayout::GBRG) ? 1 : 0; +} + +constexpr int bayerRedCol(BayerLayout layout) +{ + return (layout == BayerLayout::BGGR || layout == BayerLayout::GRBG) ? 1 : 0; +} + +struct BayerFormat +{ + BayerLayout layout; + bool is_16bit; +}; + +// Single source of truth for the Bayer encodings the image displays can +// demosaic. Returns std::nullopt for anything else — including Bayer variants +// sensor_msgs may grow later: gating UI and conversion on this whitelist +// (rather than on image_encodings::isBayer()) keeps both in lockstep with +// what the converter actually implements. +inline std::optional bayerFormatFromEncoding(const std::string & encoding) +{ + if (encoding == sensor_msgs::image_encodings::BAYER_RGGB8) { + return BayerFormat{BayerLayout::RGGB, false}; + } + if (encoding == sensor_msgs::image_encodings::BAYER_BGGR8) { + return BayerFormat{BayerLayout::BGGR, false}; + } + if (encoding == sensor_msgs::image_encodings::BAYER_GBRG8) { + return BayerFormat{BayerLayout::GBRG, false}; + } + if (encoding == sensor_msgs::image_encodings::BAYER_GRBG8) { + return BayerFormat{BayerLayout::GRBG, false}; + } + if (encoding == sensor_msgs::image_encodings::BAYER_RGGB16) { + return BayerFormat{BayerLayout::RGGB, true}; + } + if (encoding == sensor_msgs::image_encodings::BAYER_BGGR16) { + return BayerFormat{BayerLayout::BGGR, true}; + } + if (encoding == sensor_msgs::image_encodings::BAYER_GBRG16) { + return BayerFormat{BayerLayout::GBRG, true}; + } + if (encoding == sensor_msgs::image_encodings::BAYER_GRBG16) { + return BayerFormat{BayerLayout::GRBG, true}; + } + return std::nullopt; +} + +} // namespace displays +} // namespace rviz_default_plugins + +#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__IMAGE__BAYER_FORMAT_HPP_ diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_display.hpp index e0f5a8e18..cce915948 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_display.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_display.hpp @@ -94,11 +94,14 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC ImageDisplay : public rviz_common::_RosTopicDi public Q_SLOTS: virtual void updateNormalizeOptions(); virtual void updateSmoothScaling(); + virtual void updateLinearInput(); protected Q_SLOTS: virtual void subscribe(); protected: + void refreshLinearInputVisibility(); + void onEnable() override; void onDisable() override; virtual void unsubscribe(); @@ -154,7 +157,15 @@ protected Q_SLOTS: rviz_common::properties::FloatProperty * max_property_; rviz_common::properties::IntProperty * median_buffer_size_property_; rviz_common::properties::BoolProperty * smooth_scaling_property_; - bool got_float_image_; + rviz_common::properties::BoolProperty * linear_input_property_; + // True for encodings whose pixel values need a min/max range mapping before + // they can be shown as 8-bit RGB: float, single-channel 16-bit, and 16-bit + // Bayer. Controls the visibility of the Normalize Range / Min Value / + // Max Value / Median window properties. + bool has_normalizable_range_; + // True when the last received message was Bayer; + // gates visibility of the Linear Input property. + bool has_bayer_encoding_; }; } // namespace displays 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 d437d6fcb..8edb9e5a0 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 @@ -61,6 +61,17 @@ class UnsupportedImageEncoding : public std::runtime_error {} }; +// A message whose encoding is supported but whose fields are inconsistent +// with each other or with the data buffer +// (zero / oversize dimensions, bad step, truncated data). +class MalformedImageMessage : public std::runtime_error +{ +public: + explicit MalformedImageMessage(const std::string & description) + : std::runtime_error("Malformed image message: " + description) + {} +}; + struct ImageData final { ImageData( @@ -125,6 +136,9 @@ class ROSImageTexture : public ROSImageTextureIface RVIZ_DEFAULT_PLUGINS_PUBLIC void setSmoothScaling(bool enabled) override; + RVIZ_DEFAULT_PLUGINS_PUBLIC + void setLinearInput(bool enabled) override; + private: // Ensures the underlying Ogre texture matches the requested dimensions, // pixel format and smooth-scaling state, recreating it if any of those @@ -143,6 +157,8 @@ class ROSImageTexture : public ROSImageTextureIface 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 convertBayerToRGBData( + const std::string & encoding, 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); @@ -173,6 +189,10 @@ class ROSImageTexture : public ROSImageTextureIface // only briefly between setSmoothScaling and the next ensureTexture call. bool smooth_scaling_; bool tex_smooth_; + // When true, apply sRGB gamma encoding before emitting 8-bit output. + // When false, values pass through the linear rescale straight to 8-bit. + // Currently only applied by the bayer demosaicing path. + bool linear_input_; uint32_t tex_width_; uint32_t tex_height_; Ogre::PixelFormat tex_format_; 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 5d3485635..f7d66305c 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 @@ -72,6 +72,10 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC ROSImageTextureIface // no-op so that out-of-tree implementors of this interface keep building // and silently ignore the setting until they opt in. virtual void setSmoothScaling(bool /*enabled*/) {} + + // When enabled, apply sRGB gamma to converter output instead of passthrough. + // Default no-op for out-of-tree implementors. + virtual void setLinearInput(bool /*enabled*/) {} }; } // namespace displays diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/camera/camera_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/camera/camera_display.cpp index ac2889e83..df2cc467b 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/camera/camera_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/camera/camera_display.cpp @@ -444,11 +444,20 @@ void CameraDisplay::update(std::chrono::nanoseconds wall_dt, std::chrono::nanose (void) wall_dt; (void) ros_dt; try { - if (texture_->update() || force_render_) { + const bool new_image = texture_->update(); + if (new_image) { + // A new frame was converted and uploaded; clear any error a previous + // (e.g. malformed) frame may have latched. + setStatus(StatusLevel::Ok, "Image", "OK"); + } + if (new_image || force_render_) { caminfo_ok_ = updateCamera(); force_render_ = false; } - } catch (UnsupportedImageEncoding & e) { + } catch (std::exception & e) { + // UnsupportedImageEncoding, MalformedImageMessage, and also allocation + // failure on absurdly large frames - degrade to an error status rather + // than letting the exception take down the application. setStatus(StatusLevel::Error, "Image", e.what()); } } diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/image/image_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/image/image_display.cpp index bd3f5c5f0..f4518f7bf 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/image/image_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/image/image_display.cpp @@ -1,5 +1,6 @@ // Copyright (c) 2012, Willow Garage, Inc. // Copyright (c) 2017, Bosch Software Innovations GmbH. +// Copyright (c) 2026, Arne Baeyens. // All rights reserved. // // Redistribution and use in source and binary forms, with or without @@ -48,8 +49,11 @@ #include #include +#include #include +#include #include +#include #include #include #include @@ -67,12 +71,14 @@ #include "rviz_common/render_panel.hpp" #include "rviz_common/uniform_string_stream.hpp" #include "rviz_common/validate_floats.hpp" +#include "rviz_default_plugins/displays/image/bayer_format.hpp" #include "rviz_default_plugins/displays/image/get_transport_from_topic.hpp" #include "rviz_default_plugins/displays/image/ros_image_texture.hpp" #include "rviz_default_plugins/displays/image/ros_image_texture_iface.hpp" #include "rviz_rendering/material_manager.hpp" #include "rviz_rendering/render_window.hpp" #include "sensor_msgs/image_encodings.hpp" + namespace rviz_default_plugins { namespace displays @@ -105,7 +111,8 @@ ImageDisplay::ImageDisplay(std::unique_ptr texture) normalize_property_ = new rviz_common::properties::BoolProperty( "Normalize Range", true, - "If set to true, will try to estimate the range of possible values from the received images.", + "If set to true, will try to estimate the range of possible values from the received images. " + "Applies to single-channel 16-bit, float, and 16-bit Bayer encodings.", this, SLOT(updateNormalizeOptions())); min_property_ = new rviz_common::properties::FloatProperty( @@ -127,7 +134,16 @@ ImageDisplay::ImageDisplay(std::unique_ptr texture) "If disabled, sampling uses nearest-neighbour.", this, SLOT(updateSmoothScaling())); - got_float_image_ = false; + linear_input_property_ = new rviz_common::properties::BoolProperty( + "Linear input", false, + "Treat pixel values as linear light and apply sRGB encoding before display. " + "Use this for sources that output linear-response images, " + "which would otherwise appear too dark on a standard display. " + "This option only affects Bayer-encoded images.", + this, SLOT(updateLinearInput())); + + has_normalizable_range_ = false; + has_bayer_encoding_ = false; } // Need to override this method because of the new type RosTopicMultiTypeProperty @@ -142,6 +158,8 @@ void ImageDisplay::onInitialize() _RosTopicDisplay::onInitialize(); subscription_ = std::make_shared(); updateNormalizeOptions(); + refreshLinearInputVisibility(); + updateLinearInput(); setupScreenRectangle(); setupRenderPanel(); updateSmoothScaling(); @@ -325,7 +343,7 @@ void ImageDisplay::unsubscribe() void ImageDisplay::updateNormalizeOptions() { - if (got_float_image_) { + if (has_normalizable_range_) { bool normalize = normalize_property_->getBool(); normalize_property_->setHidden(false); @@ -350,6 +368,16 @@ void ImageDisplay::updateSmoothScaling() applySmoothScalingToMaterial(material_); } +void ImageDisplay::updateLinearInput() +{ + texture_->setLinearInput(linear_input_property_->getBool()); +} + +void ImageDisplay::refreshLinearInputVisibility() +{ + linear_input_property_->setHidden(!has_bayer_encoding_); +} + void ImageDisplay::applySmoothScalingToMaterial(const Ogre::MaterialPtr & material) const { if (!material) {return;} @@ -370,7 +398,11 @@ void ImageDisplay::update(std::chrono::nanoseconds wall_dt, std::chrono::nanosec (void)wall_dt; (void)ros_dt; try { - texture_->update(); + if (texture_->update()) { + // A new frame was converted and uploaded; clear any error a previous + // (e.g. malformed) frame may have latched. + setStatus(rviz_common::properties::StatusProperty::Ok, "Image", "OK"); + } // make sure the aspect ratio of the image is preserved float win_width = render_panel_->width(); @@ -391,7 +423,10 @@ void ImageDisplay::update(std::chrono::nanoseconds wall_dt, std::chrono::nanosec -1.0f * img_aspect / win_aspect, 1.0f, 1.0f * img_aspect / win_aspect, -1.0f, false); } } - } catch (UnsupportedImageEncoding & e) { + } catch (std::exception & e) { + // UnsupportedImageEncoding, MalformedImageMessage, and also allocation + // failure on absurdly large frames — degrade to an error status rather + // than letting the exception take down the application. setStatus(rviz_common::properties::StatusProperty::Error, "Image", e.what()); } } @@ -406,16 +441,29 @@ void ImageDisplay::reset() /* This is called by incomingMessage(). */ void ImageDisplay::processMessage(sensor_msgs::msg::Image::ConstSharedPtr msg) { - bool got_float_image = + // Gate the Bayer-dependent UI on the converter's own whitelist (via + // bayer_format.hpp) rather than image_encodings::isBayer(), so the + // properties only appear for encodings the demosaic actually supports. + const auto bayer_format = bayerFormatFromEncoding(msg->encoding); + + const bool has_normalizable_range = msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1 || msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1 || msg->encoding == sensor_msgs::image_encodings::TYPE_16SC1 || - msg->encoding == sensor_msgs::image_encodings::MONO16; + msg->encoding == sensor_msgs::image_encodings::MONO16 || + (bayer_format && bayer_format->is_16bit); - if (got_float_image != got_float_image_) { - got_float_image_ = got_float_image; + if (has_normalizable_range != has_normalizable_range_) { + has_normalizable_range_ = has_normalizable_range; updateNormalizeOptions(); } + + const bool is_bayer = bayer_format.has_value(); + if (is_bayer != has_bayer_encoding_) { + has_bayer_encoding_ = is_bayer; + refreshLinearInputVisibility(); + } + last_msg_ = msg; texture_->addMessage(msg); } @@ -599,9 +647,101 @@ QString formatRawBytes(const uint8_t * p, size_t n) return out; } +// Bayer 2x2 cell description: which channel sits at each of the four +// (top-left, top-right, bottom-left, bottom-right) positions, plus bit depth. +struct BayerCell +{ + char tl; + char tr; + char bl; + char br; + bool is_16bit; +}; + +// Derive the four cell corners from the shared layout table so the read-out +// cannot drift from what the demosaic in ros_image_texture.cpp implements. +std::optional bayerCellLayout(const std::string & encoding) +{ + const auto format = bayerFormatFromEncoding(encoding); + if (!format) { + return std::nullopt; + } + char corners[2][2] = {{'G', 'G'}, {'G', 'G'}}; + corners[bayerRedRow(format->layout)][bayerRedCol(format->layout)] = 'R'; + corners[1 - bayerRedRow(format->layout)][1 - bayerRedCol(format->layout)] = 'B'; + return BayerCell{corners[0][0], corners[0][1], corners[1][0], corners[1][1], format->is_16bit}; +} + +// ch is one of 'R', 'G', 'B' (the only values BayerCell holds). +constexpr const char * htmlColorForBayerChannel(char ch) +{ + if (ch == 'R') {return "#c00";} + if (ch == 'G') {return "#0a0";} + return "#06c"; +} + +QString formatBayerSensel(char ch, int value) +{ + return QString("%2:%3") + .arg(htmlColorForBayerChannel(ch)) + .arg(ch) + .arg(value); +} + +// Show the four sensel values of the 2x2 Bayer cell containing (px, py). +// The cell is anchored at (px & ~1, py & ~1); read-out positions are clamped +// to the image bounds for pixels at the edge. +QString formatBayerCellAt( + const sensor_msgs::msg::Image & msg, int px, int py, const BayerCell & cell) +{ + const int cell_x = px & ~1; + const int cell_y = py & ~1; + const int bpp = cell.is_16bit ? 2 : 1; + // Clamp the dimensions into int range first: a hostile width/height above + // INT_MAX would otherwise make max_x/max_y negative and feed std::clamp a + // lo > hi precondition violation. Reads beyond the actual buffer are still + // caught by the data-size guard in read_at below. + constexpr uint32_t kMaxInt = static_cast(std::numeric_limits::max()); + const int max_x = static_cast(std::min(msg.width, kMaxInt)) - 1; + const int max_y = static_cast(std::min(msg.height, kMaxInt)) - 1; + + auto read_at = [&](int y, int x) -> int { + y = std::clamp(y, 0, max_y); + x = std::clamp(x, 0, max_x); + const size_t offset = static_cast(y) * msg.step + + static_cast(x) * static_cast(bpp); + if (offset + static_cast(bpp) > msg.data.size()) {return 0;} + if (cell.is_16bit) { + uint16_t v = 0; + std::memcpy(&v, msg.data.data() + offset, sizeof(v)); + return v; + } + return msg.data[offset]; + }; + + const int tl = read_at(cell_y, cell_x); + const int tr = read_at(cell_y, cell_x + 1); + const int bl = read_at(cell_y + 1, cell_x); + const int br = read_at(cell_y + 1, cell_x + 1); + + return formatBayerSensel(cell.tl, tl) + " " + + formatBayerSensel(cell.tr, tr) + " | " + + formatBayerSensel(cell.bl, bl) + " " + + formatBayerSensel(cell.br, br); +} + QString formatPixel(const sensor_msgs::msg::Image & msg, int px, int py) { QString prefix = "[" + QString::fromStdString(msg.encoding) + "] "; + + // Bayer encodings: show the four raw sensel values of the 2x2 cell + // containing (px, py), each labelled by channel. The on-screen pixel comes + // from a bilinear interpolation of these (and their neighbours); showing + // them is honest about what the sensor actually recorded. + if (const auto cell = bayerCellLayout(msg.encoding)) { + return prefix + formatBayerCellAt(msg, px, py, *cell); + } + const size_t pixel_size = pixelSizeForEncoding(msg.encoding); // Even when pixel_size is 0 (encoding not decoded inline) try to show the 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 c6d9e748e..c012388a5 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 @@ -1,4 +1,5 @@ // Copyright (c) 2009, Willow Garage, Inc. +// Copyright (c) 2026, Arne Baeyens. // All rights reserved. // // Redistribution and use in source and binary forms, with or without @@ -31,11 +32,15 @@ #include "rviz_default_plugins/displays/image/ros_image_texture.hpp" #include +#include +#include +#include #include #include #include #include #include +#include #include #include #include @@ -53,6 +58,7 @@ #include "rviz_common/logging.hpp" #include "rviz_common/uniform_string_stream.hpp" +#include "rviz_default_plugins/displays/image/bayer_format.hpp" namespace rviz_default_plugins { @@ -66,6 +72,7 @@ ROSImageTexture::ROSImageTexture() median_frames_(5), smooth_scaling_(false), tex_smooth_(false), + linear_input_(false), tex_width_(0), tex_height_(0), tex_format_(Ogre::PF_UNKNOWN) @@ -169,6 +176,19 @@ void ROSImageTexture::setSmoothScaling(bool enabled) } } +void ROSImageTexture::setLinearInput(bool enabled) +{ + std::lock_guard lock(mutex_); + if (linear_input_ == enabled) { + return; + } + linear_input_ = enabled; + // Re-arm so latched topics pick up the change without a new publish. + if (current_image_) { + new_image_ = true; + } +} + // Bytes per pixel for encodings that have a fixed linear row layout. Returns // 0 for encodings whose per-row layout is non-trivial (YUV 4:2:2 packed, NV12 // planar) — those are handled explicitly by their converters using stride. @@ -408,6 +428,263 @@ static void imageConvertYUYVToRGB( } } +// Bayer demosaic helpers (file-scope). Bilinear interpolation, hand-rolled +// to avoid an OpenCV / cv_bridge dependency. Output is 8-bit RGB; the caller +// decides whether to apply sRGB gamma (via linear_input_) or emit +// the demosaiced linear values directly. +// Note: the encoding -> layout / bit-depth mapping lives in bayer_format.hpp, +// shared with the encoding gates and pixel read-out in image_display.cpp. + +// Piecewise sRGB transfer function (linear -> sRGB), input in [0, 1]. +static double srgbEncode(double linear) +{ + if (linear <= 0.0031308) { + return 12.92 * linear; + } + return 1.055 * std::pow(linear, 1.0 / 2.4) - 0.055; +} + +// 256-entry LUT: 8-bit linear -> 8-bit sRGB. Constructed once on first use. +static const std::array & srgbLut8() +{ + static const std::array lut = []() { + std::array t{}; + for (int i = 0; i < 256; ++i) { + const double s = std::clamp(srgbEncode(i / 255.0), 0.0, 1.0); + t[static_cast(i)] = static_cast(std::lround(s * 255.0)); + } + return t; + }(); + return lut; +} + +// 65536-entry LUT: 16-bit linear -> 8-bit sRGB. 64 KiB; fits in L2; built once. +static const std::array & srgbLut16() +{ + static const std::array lut = []() { + std::array t{}; + for (int i = 0; i < 65536; ++i) { + const double s = std::clamp(srgbEncode(i / 65535.0), 0.0, 1.0); + t[static_cast(i)] = static_cast(std::lround(s * 255.0)); + } + return t; + }(); + return lut; +} + +// Demosaic implementation. T is the input pixel type (uint8_t or uint16_t). +// R_DY / R_DX encode the layout (position of R within the 2x2 cell). +// `q` is a callable that maps an integer linear value to the final uint8_t +// output; the caller decides whether that mapping applies sRGB gamma or is +// a plain clamp/rescale. +template +static void demosaicBayerImpl( + uint8_t * dst_rgb, + const T * src_mosaic, + uint32_t height, + uint32_t width, + uint32_t stride_in_pixels, + Quantize && q) +{ + constexpr int B_DY = 1 - R_DY; + constexpr int B_DX = 1 - R_DX; + + auto write_pixel = [&](uint32_t y, uint32_t x, int r_lin, int g_lin, int b_lin) { + const size_t out_idx = (static_cast(y) * width + x) * 3; + dst_rgb[out_idx + 0] = q(r_lin); + dst_rgb[out_idx + 1] = q(g_lin); + dst_rgb[out_idx + 2] = q(b_lin); + }; + + // Border-pixel handler: bounds-checked neighbour access, averages over the + // count of valid neighbours (not a fixed denominator). Used for the 1-pixel + // border (and for the whole image if w < 3 or h < 3). + // + // Edge pixels use clamp-to-edge for missing neighbours. For a G-position + // pixel on the boundary, the clamped neighbour is itself a G (not R or B), + // which is a small chromatic miscolouring of edge pixels. Acceptable for + // a display path; mirror-padding would complicate the hot loop for an + // artifact that only shows up on thin sub-pixel edge features. + auto handle_border = [&](uint32_t y, uint32_t x) { + auto p = [&](int32_t dy, int32_t dx) -> int { + int32_t yy = std::clamp( + static_cast(y) + dy, int32_t{0}, + static_cast(height) - 1); + int32_t xx = std::clamp( + static_cast(x) + dx, int32_t{0}, + static_cast(width) - 1); + return src_mosaic[static_cast(yy) * stride_in_pixels + + static_cast(xx)]; + }; + const bool has_n = y > 0; + const bool has_s = y + 1 < height; + const bool has_w = x > 0; + const bool has_e = x + 1 < width; + + auto avg_cardinal = [&]() -> int { + int sum = 0; + int count = 0; + if (has_n) {sum += p(-1, 0); ++count;} + if (has_s) {sum += p(1, 0); ++count;} + if (has_w) {sum += p(0, -1); ++count;} + if (has_e) {sum += p(0, 1); ++count;} + return count ? (sum + count / 2) / count : 0; + }; + auto avg_diagonal = [&]() -> int { + int sum = 0; + int count = 0; + if (has_n && has_w) {sum += p(-1, -1); ++count;} + if (has_n && has_e) {sum += p(-1, 1); ++count;} + if (has_s && has_w) {sum += p(1, -1); ++count;} + if (has_s && has_e) {sum += p(1, 1); ++count;} + return count ? (sum + count / 2) / count : 0; + }; + auto avg_horizontal = [&]() -> int { + int sum = 0; + int count = 0; + if (has_w) {sum += p(0, -1); ++count;} + if (has_e) {sum += p(0, 1); ++count;} + return count ? (sum + count / 2) / count : 0; + }; + auto avg_vertical = [&]() -> int { + int sum = 0; + int count = 0; + if (has_n) {sum += p(-1, 0); ++count;} + if (has_s) {sum += p(1, 0); ++count;} + return count ? (sum + count / 2) / count : 0; + }; + + const int yp = static_cast(y & 1u); + const int xp = static_cast(x & 1u); + int r_lin; + int g_lin; + int b_lin; + + if (yp == R_DY && xp == R_DX) { + r_lin = p(0, 0); + g_lin = avg_cardinal(); + b_lin = avg_diagonal(); + } else if (yp == B_DY && xp == B_DX) { + r_lin = avg_diagonal(); + g_lin = avg_cardinal(); + b_lin = p(0, 0); + } else { + g_lin = p(0, 0); + if (yp == R_DY) { + r_lin = avg_horizontal(); + b_lin = avg_vertical(); + } else { + r_lin = avg_vertical(); + b_lin = avg_horizontal(); + } + } + write_pixel(y, x, r_lin, g_lin, b_lin); + }; + + if (height < 3 || width < 3) { + for (uint32_t y = 0; y < height; ++y) { + for (uint32_t x = 0; x < width; ++x) { + handle_border(y, x); + } + } + return; + } + + // Border passes. + for (uint32_t x = 0; x < width; ++x) { + handle_border(0, x); + } + for (uint32_t x = 0; x < width; ++x) { + handle_border(height - 1, x); + } + for (uint32_t y = 1; y + 1 < height; ++y) { + handle_border(y, 0); + } + for (uint32_t y = 1; y + 1 < height; ++y) { + handle_border(y, width - 1); + } + + // Interior. No bounds checks, fixed 2-tap or 4-tap averages. + for (uint32_t y = 1; y + 1 < height; ++y) { + const T * row_m1 = src_mosaic + static_cast(y - 1) * stride_in_pixels; + const T * row_0 = src_mosaic + static_cast(y) * stride_in_pixels; + const T * row_p1 = src_mosaic + static_cast(y + 1) * stride_in_pixels; + const int yp = static_cast(y & 1u); + + for (uint32_t x = 1; x + 1 < width; ++x) { + const int p_nw = row_m1[x - 1]; + const int p_n = row_m1[x]; + const int p_ne = row_m1[x + 1]; + const int p_w = row_0[x - 1]; + const int p_c = row_0[x]; + const int p_e = row_0[x + 1]; + const int p_sw = row_p1[x - 1]; + const int p_s = row_p1[x]; + const int p_se = row_p1[x + 1]; + + const int xp = static_cast(x & 1u); + int r_lin; + int g_lin; + int b_lin; + + if (yp == R_DY && xp == R_DX) { + r_lin = p_c; + g_lin = (p_n + p_s + p_w + p_e + 2) / 4; + b_lin = (p_nw + p_ne + p_sw + p_se + 2) / 4; + } else if (yp == B_DY && xp == B_DX) { + r_lin = (p_nw + p_ne + p_sw + p_se + 2) / 4; + g_lin = (p_n + p_s + p_w + p_e + 2) / 4; + b_lin = p_c; + } else { + g_lin = p_c; + if (yp == R_DY) { + r_lin = (p_w + p_e + 1) / 2; + b_lin = (p_n + p_s + 1) / 2; + } else { + r_lin = (p_n + p_s + 1) / 2; + b_lin = (p_w + p_e + 1) / 2; + } + } + write_pixel(y, x, r_lin, g_lin, b_lin); + } + } +} + +// Dispatch the layout to the templated implementation. +template +static void demosaicBayer( + uint8_t * dst_rgb, + const T * src_mosaic, + BayerLayout layout, + uint32_t height, + uint32_t width, + uint32_t stride_in_pixels, + Quantize && q) +{ + switch (layout) { + case BayerLayout::RGGB: + demosaicBayerImpl( + dst_rgb, src_mosaic, height, width, stride_in_pixels, + std::forward(q)); + return; + case BayerLayout::BGGR: + demosaicBayerImpl( + dst_rgb, src_mosaic, height, width, stride_in_pixels, + std::forward(q)); + return; + case BayerLayout::GBRG: + demosaicBayerImpl( + dst_rgb, src_mosaic, height, width, stride_in_pixels, + std::forward(q)); + return; + case BayerLayout::GRBG: + demosaicBayerImpl( + dst_rgb, src_mosaic, height, width, stride_in_pixels, + std::forward(q)); + return; + } +} + ImageData::ImageData( Ogre::PixelFormat pixformat, const uint8_t * data_ptr, @@ -588,6 +865,132 @@ ROSImageTexture::convertNV12ToRGBData(const uint8_t * data_ptr, size_t data_size return ImageData(Ogre::PF_BYTE_RGB, new_data, new_size_in_bytes, true); } +ImageData +ROSImageTexture::convertBayerToRGBData( + const std::string & encoding, const uint8_t * data_ptr, size_t data_size_in_bytes) +{ + const auto format = bayerFormatFromEncoding(encoding); + if (!format) { + throw UnsupportedImageEncoding(encoding); + } + + // Input validation: untrusted ROS publishers can send malformed messages. + // Catch undersized / inconsistent inputs upfront so the demosaic loops can + // index without bounds checks. + const uint32_t bytes_per_pixel = format->is_16bit ? 2u : 1u; + // 32768 = 2^15 per dimension and 2^28 (~268 megapixel) in total: + // comfortably larger than any real camera while bounding the RGB output + // allocation (width * height * 3, ~800 MB worst case, below the 32-bit + // size_t ceiling of ~4.3 * 10^9). + constexpr uint32_t kMaxDimension = 32768u; + constexpr uint64_t kMaxPixels = uint64_t{1} << 28; + if (width_ == 0 || height_ == 0) { + throw MalformedImageMessage( + "zero image dimension (" + + std::to_string(width_) + "x" + std::to_string(height_) + ")"); + } + if (width_ > kMaxDimension || height_ > kMaxDimension || + static_cast(width_) * height_ > kMaxPixels) + { + throw MalformedImageMessage( + "image dimensions too large (" + + std::to_string(width_) + "x" + std::to_string(height_) + ")"); + } + // update() repacks padded rows before conversion, so a step mismatch here + // means a malformed message. Requiring exact packing also pins down the + // precondition the 16-bit min/max scan below relies on. + if (stride_ != width_ * bytes_per_pixel) { + throw MalformedImageMessage( + "step " + std::to_string(stride_) + " does not match width " + + std::to_string(width_) + " at " + std::to_string(bytes_per_pixel) + + " byte(s) per pixel"); + } + if (data_size_in_bytes < static_cast(height_) * stride_) { + throw MalformedImageMessage( + "data size " + std::to_string(data_size_in_bytes) + " below the " + + std::to_string(static_cast(height_) * stride_) + + " bytes implied by height and step"); + } + + const size_t out_size = static_cast(width_) * height_ * 3u; + auto out_buf = std::make_unique(out_size); + + if (!format->is_16bit) { + // 8-bit Bayer: the demosaiced linear value is already in [0, 255], so + // when linear_input_ is true we apply the 256-entry sRGB LUT; + // when false we clamp and emit the linear value directly. + if (linear_input_) { + const auto & lut = srgbLut8(); + demosaicBayer( + out_buf.get(), data_ptr, format->layout, height_, width_, stride_, + [&lut](int v) -> uint8_t { + return lut[static_cast(std::clamp(v, 0, 255))]; + }); + } else { + demosaicBayer( + out_buf.get(), data_ptr, format->layout, height_, width_, stride_, + [](int v) -> uint8_t { + return static_cast(std::clamp(v, 0, 255)); + }); + } + } else { + // 16-bit Bayer. The validation above guarantees tightly packed rows, so + // min/max is a plain contiguous scan. + const uint16_t * src16 = reinterpret_cast(data_ptr); + const uint32_t stride_in_pixels = stride_ / bytes_per_pixel; + + double min_value; + double max_value; + getMinimalAndMaximalValueToNormalize( + src16, static_cast(width_) * height_, min_value, max_value); + + const double range = max_value - min_value; + + if (range > 0.0 && std::isfinite(range)) { + // Demosaiced 16-bit averages stay within [0, 65535], so fold the + // rescale (and, for linear input, the sRGB transfer) into a per-frame + // 16-bit -> 8-bit LUT: the inner loop then does one table lookup per + // channel instead of floating-point math. Building the 65536 entries + // once per frame amortizes over the pixels. + const double offset = min_value; + std::vector quantize_lut(65536); + if (linear_input_) { + // Rescale at 16-bit precision before the sRGB transfer so the steep + // toe of the curve isn't banded by 8-bit quantisation. The + // 16-bit-precision claim holds when the user-supplied range stays + // within 65535; wider ranges reduce precision proportionally. + const auto & srgb = srgbLut16(); + const double scale = 65535.0 / range; + for (int i = 0; i < 65536; ++i) { + const double scaled = (static_cast(i) - offset) * scale; + quantize_lut[static_cast(i)] = srgb[static_cast( + std::lround(std::clamp(scaled, 0.0, 65535.0)))]; + } + } else { + // No gamma: rescale linearly straight into the 8-bit range. + const double scale = 255.0 / range; + for (int i = 0; i < 65536; ++i) { + const double scaled = (static_cast(i) - offset) * scale; + quantize_lut[static_cast(i)] = static_cast( + std::lround(std::clamp(scaled, 0.0, 255.0))); + } + } + demosaicBayer( + out_buf.get(), src16, format->layout, height_, width_, stride_in_pixels, + [&quantize_lut](int v) -> uint8_t { + return quantize_lut[static_cast(v)]; + }); + } else { + // No dynamic range in the input; there is no meaningful rescale, so + // emit uniform black by convention (matches convertTo8bit). + std::fill_n(out_buf.get(), out_size, uint8_t{0}); + } + } + + uint8_t * raw = out_buf.release(); + return ImageData(Ogre::PF_BYTE_RGB, raw, out_size, true); +} + ImageData ROSImageTexture::setFormatAndNormalizeDataIfNecessary( const std::string & encoding, const uint8_t * data_ptr, size_t data_size_in_bytes) @@ -621,7 +1024,7 @@ ROSImageTexture::setFormatAndNormalizeDataIfNecessary( { 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); + return convertBayerToRGBData(encoding, data_ptr, data_size_in_bytes); } else if (encoding == sensor_msgs::image_encodings::TYPE_32FC1) { return convertTo8bit(data_ptr, data_size_in_bytes); } else if (encoding == sensor_msgs::image_encodings::UYVY) { diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/image/image_display_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/image/image_display_test.cpp index d3b7177f9..2f4c1b03d 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/image/image_display_test.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/image/image_display_test.cpp @@ -149,6 +149,19 @@ TEST_F(ImageDisplayTestFixture, initialize_propagates_smooth_scaling_to_texture) imageDisplay.initialize(context_.get()); } +TEST_F(ImageDisplayTestFixture, initialize_propagates_linear_input_default_to_texture) { + auto panelDockWidget = new rviz_common::PanelDockWidget("panelDockWidget"); + EXPECT_CALL(*window_manager_, addPane(_, _, _, _)).WillOnce(Return(panelDockWidget)); + EXPECT_CALL(*context_, getFixedFrame()).WillOnce(Return("")); + + // Default is unchecked; the display must push `false` down exactly once + // during initialize(). + EXPECT_CALL(*texture_, setLinearInput(false)).Times(1); + + ImageDisplay imageDisplay(std::move(texture_)); + imageDisplay.initialize(context_.get()); +} + int main(int argc, char ** argv) { QApplication app(argc, argv); 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 b08cd5900..492604948 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 @@ -56,6 +56,7 @@ class MockROSImageTexture : public rviz_default_plugins::displays::ROSImageTextu MOCK_METHOD3(setNormalizeFloatImage, void(bool normalize, double min, double max)); MOCK_METHOD1(setMedianFrames, void(unsigned median_frames)); MOCK_METHOD1(setSmoothScaling, void(bool enabled)); + MOCK_METHOD1(setLinearInput, void(bool enabled)); }; #endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__IMAGE__MOCK_ROS_IMAGE_TEXTURE_HPP_ diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/image/ros_image_texture_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/image/ros_image_texture_test.cpp index 7f6bba8d6..01a19d2d1 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/image/ros_image_texture_test.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/image/ros_image_texture_test.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2017, Bosch Software Innovations GmbH. +// Copyright (c) 2026, Arne Baeyens. // All rights reserved. // // Redistribution and use in source and binary forms, with or without @@ -31,8 +32,12 @@ #include #include +#include #include +#include +#include #include +#include #include #include // NOLINT @@ -52,8 +57,12 @@ class RosImageTextureTestFixture : public ::testing::Test protected: static void SetUpTestCase() { - testing_environment_ = std::make_shared(); - testing_environment_->setUpOgreTestEnvironment(); + // Idempotent: Ogre is a singleton, and parameterised subclasses each get + // their own SetUpTestCase call. Guard against re-initialising it. + if (!testing_environment_) { + testing_environment_ = std::make_shared(); + testing_environment_->setUpOgreTestEnvironment(); + } } static std::shared_ptr testing_environment_; @@ -246,3 +255,706 @@ TEST_F(RosImageTextureTestFixture, update_with_smooth_scaling_writes_new_image_t mip1->blitToMemory(dst); ASSERT_TRUE(std::any_of(mip1_data.begin(), mip1_data.end(), [](uint8_t v) {return v != 0;})); } + +// ----- Bayer demosaic tests ----- +// +// The mosaic layout determines which sensel color sits at which 2x2 position: +// RGGB -> (0,0)=R, (0,1)=G, (1,0)=G, (1,1)=B +// BGGR -> (0,0)=B, (0,1)=G, (1,0)=G, (1,1)=R +// GBRG -> (0,0)=G, (0,1)=B, (1,0)=R, (1,1)=G +// GRBG -> (0,0)=G, (0,1)=R, (1,0)=B, (1,1)=G +// Tests build a layout-specific mosaic representing a known scene and verify +// that the demosaiced output matches the expectation. Per-layout construction +// is required: a mosaic that "means red" under one layout means a different +// color under another, so a layout-swap bug surfaces as wrong output colors. + +namespace +{ + +struct LayoutOffsets +{ + int r_y; + int r_x; + int b_y; + int b_x; +}; + +LayoutOffsets offsetsFor(const std::string & encoding) +{ + namespace enc = sensor_msgs::image_encodings; + if (encoding == enc::BAYER_RGGB8 || encoding == enc::BAYER_RGGB16) {return {0, 0, 1, 1};} + if (encoding == enc::BAYER_BGGR8 || encoding == enc::BAYER_BGGR16) {return {1, 1, 0, 0};} + if (encoding == enc::BAYER_GBRG8 || encoding == enc::BAYER_GBRG16) {return {1, 0, 0, 1};} + if (encoding == enc::BAYER_GRBG8 || encoding == enc::BAYER_GRBG16) {return {0, 1, 1, 0};} + // Helper is only called with known Bayer encodings. Anything else is a + // test-side bug. + ADD_FAILURE() << "offsetsFor() called with unknown encoding: " << encoding; + return {0, 0, 1, 1}; +} + +// Build a mosaic of a uniform scene with given (R, G, B) channel intensities, +// for the given Bayer layout. The mosaic places the R value at R positions, +// the G value at G positions, and the B value at B positions — matching what +// a real sensor records for a uniform scene. T is the per-sensel storage type +// (uint8_t for 8-bit encodings, uint16_t for 16-bit). +template +std::vector buildUniformMosaic( + uint32_t width, uint32_t height, + T r_val, T g_val, T b_val, + const std::string & encoding) +{ + const LayoutOffsets off = offsetsFor(encoding); + std::vector data(static_cast(width) * height * sizeof(T)); + for (uint32_t y = 0; y < height; ++y) { + for (uint32_t x = 0; x < width; ++x) { + const int yp = static_cast(y & 1u); + const int xp = static_cast(x & 1u); + T v; + if (yp == off.r_y && xp == off.r_x) { + v = r_val; + } else if (yp == off.b_y && xp == off.b_x) { + v = b_val; + } else { + v = g_val; + } + const size_t idx = (static_cast(y) * width + x) * sizeof(T); + std::memcpy(data.data() + idx, &v, sizeof(T)); + } + } + return data; +} + +sensor_msgs::msg::Image::SharedPtr makeImage( + uint32_t width, uint32_t height, + const std::string & encoding, + std::vector data, + uint32_t bytes_per_pixel) +{ + auto msg = std::make_shared(); + msg->width = width; + msg->height = height; + msg->encoding = encoding; + msg->step = width * bytes_per_pixel; + msg->data = std::move(data); + return msg; +} + +// Read the texture content back as a flat RGB byte vector (size = h * w * 3). +// Ogre may store the texture internally as RGBA depending on backend/format; +// strip the alpha channel here so callers can index uniformly with 3 bytes +// per pixel. +std::vector readTextureRGB(ROSImageTexture & texture) +{ + Ogre::TexturePtr ogre_texture = texture.getTexture(); + Ogre::Image image; + ogre_texture->convertToImage(image); + const uint8_t * data = image.getData(); + const size_t total = image.getSize(); + const uint32_t w = image.getWidth(); + const uint32_t h = image.getHeight(); + const size_t pixels = static_cast(w) * h; + if (pixels == 0) {return {};} + const size_t bpp = total / pixels; + + std::vector out(pixels * 3); + for (size_t i = 0; i < pixels; ++i) { + out[i * 3 + 0] = data[i * bpp + 0]; + out[i * 3 + 1] = data[i * bpp + 1]; + out[i * 3 + 2] = data[i * bpp + 2]; + } + return out; +} + +// Index helper for an h*w*3 flat buffer. +size_t rgbIndex(uint32_t y, uint32_t x, uint32_t width) +{ + return (static_cast(y) * width + x) * 3; +} + +// Expected sRGB output (8-bit) for a normalized linear input +uint8_t srgbEncodeByte(double linear) +{ + double s; + if (linear <= 0.0031308) { + s = 12.92 * linear; + } else { + s = 1.055 * std::pow(linear, 1.0 / 2.4) - 0.055; + } + if (s < 0.0) {s = 0.0;} + if (s > 1.0) {s = 1.0;} + return static_cast(std::lround(s * 255.0)); +} + +} // namespace + +// Per-layout discriminating test helper: a mosaic where only `channel` is +// non-zero (channel 0 = R, 1 = G, 2 = B) must produce output where only that +// channel is populated (after sRGB encoding). A layout-swap bug surfaces as +// swapped output channels; per-layout mosaic construction makes it visible. +void checkPureChannelDemosaicSrgb(const std::string & encoding, int channel) +{ + ASSERT_GE(channel, 0); + ASSERT_LE(channel, 2); + const uint32_t w = 8; + const uint32_t h = 8; + const uint8_t v = 200; // mid-range so sRGB encoding is meaningful + + const uint8_t r_val = (channel == 0) ? v : 0; + const uint8_t g_val = (channel == 1) ? v : 0; + const uint8_t b_val = (channel == 2) ? v : 0; + + auto msg = makeImage(w, h, encoding, + buildUniformMosaic(w, h, r_val, g_val, b_val, encoding), 1); + + ROSImageTexture texture; + texture.setLinearInput(true); + texture.addMessage(msg); + ASSERT_TRUE(texture.update()) << "for encoding " << encoding; + + const std::vector rgb = readTextureRGB(texture); + ASSERT_EQ(rgb.size(), static_cast(w) * h * 3) << "for " << encoding; + + const uint8_t expected = srgbEncodeByte(v / 255.0); + + // Check interior pixels. Edge / corner pixels are excluded because the + // border passes interpolate over fewer neighbours and the result is less + // exact for non-trivial neighbour patterns. + for (uint32_t y = 2; y + 2 < h; ++y) { + for (uint32_t x = 2; x + 2 < w; ++x) { + const size_t i = rgbIndex(y, x, w); + for (int c = 0; c < 3; ++c) { + if (c == channel) { + EXPECT_NEAR(rgb[i + c], expected, 2) + << encoding << " channel=" << c << " at (" << y << "," << x << ")"; + } else { + EXPECT_EQ(rgb[i + c], 0u) + << encoding << " channel=" << c << " at (" << y << "," << x << ")"; + } + } + } + } +} + +// Bayer 8-bit sRGB path, parameterised over (layout, channel). Each +// combination becomes a named test in the failure output. +class Bayer8bitSrgbPureChannelTestFixture + : public RosImageTextureTestFixture, + public ::testing::WithParamInterface> +{}; + +TEST_P(Bayer8bitSrgbPureChannelTestFixture, pure_channel_output_matches_expectation) { + const auto & [encoding, channel] = GetParam(); + checkPureChannelDemosaicSrgb(encoding, channel); +} + +INSTANTIATE_TEST_SUITE_P( + AllLayoutsAndChannels, + Bayer8bitSrgbPureChannelTestFixture, + ::testing::Combine( + ::testing::Values( + sensor_msgs::image_encodings::BAYER_RGGB8, + sensor_msgs::image_encodings::BAYER_BGGR8, + sensor_msgs::image_encodings::BAYER_GBRG8, + sensor_msgs::image_encodings::BAYER_GRBG8), + ::testing::Values(0, 2) // R and B are the discriminating channels +)); + +// 16-bit path: with Normalize Range = false and Max Value set to the input +// maximum, a "pure red" 16-bit mosaic produces pure red 8-bit output. +// Parameterised over the four layouts via four TEST_F entries: the demosaic +// implementation is parameterised on BayerLayout, so one test per layout is +// enough to catch a dispatch / channel-mapping bug at 16-bit. +void checkBayer16WithFixedMax(const std::string & encoding) +{ + const uint32_t w = 8; + const uint32_t h = 8; + const uint16_t r = 1023; // simulate 10-bit-in-16 camera at full white + + auto msg = makeImage(w, h, encoding, + buildUniformMosaic(w, h, r, /*g*/ 0, /*b*/ 0, encoding), 2); + + ROSImageTexture texture; + texture.setLinearInput(true); + texture.setNormalizeFloatImage(false, 0.0, 1023.0); + texture.addMessage(msg); + ASSERT_TRUE(texture.update()) << "encoding " << encoding; + + const std::vector rgb = readTextureRGB(texture); + const uint8_t expected_r = 255; // 1023 -> normalized 1.0 -> sRGB(1.0) = 255 + + for (uint32_t y = 2; y + 2 < h; ++y) { + for (uint32_t x = 2; x + 2 < w; ++x) { + const size_t i = rgbIndex(y, x, w); + EXPECT_NEAR(rgb[i + 0], expected_r, 1) << encoding << " (" << y << "," << x << ")"; + EXPECT_EQ(rgb[i + 1], 0u) << encoding << " (" << y << "," << x << ")"; + EXPECT_EQ(rgb[i + 2], 0u) << encoding << " (" << y << "," << x << ")"; + } + } +} + +class Bayer16bitSrgbFixedMaxTestFixture + : public RosImageTextureTestFixture, + public ::testing::WithParamInterface +{}; + +TEST_P(Bayer16bitSrgbFixedMaxTestFixture, pure_red_output_matches_expectation) { + checkBayer16WithFixedMax(GetParam()); +} + +INSTANTIATE_TEST_SUITE_P( + AllLayouts, + Bayer16bitSrgbFixedMaxTestFixture, + ::testing::Values( + sensor_msgs::image_encodings::BAYER_RGGB16, + sensor_msgs::image_encodings::BAYER_BGGR16, + sensor_msgs::image_encodings::BAYER_GBRG16, + sensor_msgs::image_encodings::BAYER_GRBG16)); + +// 16-bit path: a mosaic at half the user-specified Max Value (10-bit half- +// white = 511) should produce sRGB(0.5) ~= 188, not 128 (which would be the +// result of sRGB-then-scale instead of scale-then-sRGB). +TEST_F(RosImageTextureTestFixture, bayer_16bit_srgb_after_scaling) { + const std::string encoding = sensor_msgs::image_encodings::BAYER_RGGB16; + const uint32_t w = 8; + const uint32_t h = 8; + const uint16_t r = 511; // ~half of 1023 + + auto msg = makeImage(w, h, encoding, + buildUniformMosaic(w, h, r, /*g*/ 0, /*b*/ 0, encoding), 2); + + ROSImageTexture texture; + texture.setLinearInput(true); + texture.setNormalizeFloatImage(false, 0.0, 1023.0); + texture.addMessage(msg); + ASSERT_TRUE(texture.update()); + + const std::vector rgb = readTextureRGB(texture); + const uint8_t expected_r = srgbEncodeByte(511.0 / 1023.0); // ~= 188 + + // The expected ~188 also discriminates against sRGB-then-scale, which + // would produce ~2/255 (sRGB(511/65535) * 1023/65535). + for (uint32_t y = 2; y + 2 < h; ++y) { + for (uint32_t x = 2; x + 2 < w; ++x) { + const size_t i = rgbIndex(y, x, w); + EXPECT_NEAR(rgb[i + 0], expected_r, 2) << "at (" << y << "," << x << ")"; + } + } +} + +// 16-bit path with Min Value (black point) > 0: with Min=100, Max=1100, an +// input of 100 should produce 0. +TEST_F(RosImageTextureTestFixture, bayer_16bit_input_at_min_value_maps_to_black) { + const std::string encoding = sensor_msgs::image_encodings::BAYER_RGGB16; + const uint32_t w = 8; + const uint32_t h = 8; + + ROSImageTexture texture; + texture.setNormalizeFloatImage(false, 100.0, 1100.0); + + auto msg = makeImage(w, h, encoding, + buildUniformMosaic(w, h, 100, 100, 100, encoding), 2); + texture.addMessage(msg); + ASSERT_TRUE(texture.update()); + const std::vector rgb = readTextureRGB(texture); + for (uint32_t y = 2; y + 2 < h; ++y) { + for (uint32_t x = 2; x + 2 < w; ++x) { + const size_t i = rgbIndex(y, x, w); + EXPECT_EQ(rgb[i + 0], 0u); + EXPECT_EQ(rgb[i + 1], 0u); + EXPECT_EQ(rgb[i + 2], 0u); + } + } +} + +// Counterpart to the black-point test: an input at Max Value (1100) should +// produce 255. +TEST_F(RosImageTextureTestFixture, bayer_16bit_input_at_max_value_maps_to_white) { + const std::string encoding = sensor_msgs::image_encodings::BAYER_RGGB16; + const uint32_t w = 8; + const uint32_t h = 8; + + ROSImageTexture texture; + texture.setNormalizeFloatImage(false, 100.0, 1100.0); + + auto msg = makeImage(w, h, encoding, + buildUniformMosaic(w, h, 1100, 1100, 1100, encoding), 2); + texture.addMessage(msg); + ASSERT_TRUE(texture.update()); + const std::vector rgb = readTextureRGB(texture); + for (uint32_t y = 2; y + 2 < h; ++y) { + for (uint32_t x = 2; x + 2 < w; ++x) { + const size_t i = rgbIndex(y, x, w); + EXPECT_EQ(rgb[i + 0], 255u); + EXPECT_EQ(rgb[i + 1], 255u); + EXPECT_EQ(rgb[i + 2], 255u); + } + } +} + +// sRGB transfer-function anchor points, verified end-to-end via the demosaic +// output rather than by cross-checking the test-side reference. Multiple +// anchors catch a wrong-exponent bug that a single midpoint could miss. +// Note: the piecewise linear segment (linear <= 0.0031308) is not exercised +// here — 1/255 > 0.0031308, so 8-bit input can't reach it, and the existing +// 16-bit tests all sit far above the knee. +TEST_F(RosImageTextureTestFixture, srgb_transfer_anchor_points_via_demosaic_output) { + const std::string encoding = sensor_msgs::image_encodings::BAYER_RGGB8; + const uint32_t w = 6; + const uint32_t h = 6; + + struct Anchor + { + uint8_t input; + uint8_t expected; + const char * name; + }; + // Expected values follow IEC 61966-2-1 evaluated at input / 255.0. + const std::vector anchors = { + {0, 0, "black"}, + {1, 13, "near knee"}, // exponent branch, near the piecewise boundary + {128, 188, "midtone"}, // overall exponent/gain/offset + {255, 255, "white"}, + }; + + for (const auto & a : anchors) { + std::vector data(static_cast(w) * h, a.input); + auto msg = makeImage(w, h, encoding, std::move(data), 1); + + ROSImageTexture texture; + texture.setLinearInput(true); + texture.addMessage(msg); + ASSERT_TRUE(texture.update()) << a.name; + + const std::vector rgb = readTextureRGB(texture); + for (uint32_t y = 2; y + 2 < h; ++y) { + for (uint32_t x = 2; x + 2 < w; ++x) { + const size_t i = rgbIndex(y, x, w); + EXPECT_NEAR(rgb[i + 0], a.expected, 1) << a.name; + EXPECT_NEAR(rgb[i + 1], a.expected, 1) << a.name; + EXPECT_NEAR(rgb[i + 2], a.expected, 1) << a.name; + } + } + } +} + +// Default (Treat as linear = false): 8-bit Bayer is passed through the linear +// rescale straight to 8-bit — no sRGB gamma is applied. A pure-red mosaic +// with r = 200 must therefore produce output r = 200 exactly (not sRGB(200) +// = ~219), which discriminates the default from the sRGB-encoded path. +// One layout is enough — layout dispatch is exercised end-to-end by the +// 8-bit sRGB matrix, and the quantizer choice (linear vs sRGB) is orthogonal +// to the layout template parameter in demosaicBayerImpl. +TEST_F(RosImageTextureTestFixture, bayer_8bit_default_is_linear_passthrough) { + const std::string encoding = sensor_msgs::image_encodings::BAYER_RGGB8; + const uint32_t w = 8; + const uint32_t h = 8; + const uint8_t r = 200; + + auto msg = makeImage(w, h, encoding, + buildUniformMosaic(w, h, r, /*g*/ 0, /*b*/ 0, encoding), 1); + + ROSImageTexture texture; // linear_input_ defaults to false + texture.addMessage(msg); + ASSERT_TRUE(texture.update()); + + const std::vector rgb = readTextureRGB(texture); + for (uint32_t y = 2; y + 2 < h; ++y) { + for (uint32_t x = 2; x + 2 < w; ++x) { + const size_t i = rgbIndex(y, x, w); + EXPECT_EQ(rgb[i + 0], r) << "at (" << y << "," << x << ")"; + EXPECT_EQ(rgb[i + 1], 0u) << "at (" << y << "," << x << ")"; + EXPECT_EQ(rgb[i + 2], 0u) << "at (" << y << "," << x << ")"; + } + } +} + +// Default (Treat as linear = false): 16-bit Bayer is linearly rescaled to +// 8-bit — no gamma. A mid-range input (r = 511 out of Max = 1023) must +// produce r ~= 127, not sRGB(511/1023) ~= 188. Linear-mode counterpart to +// bayer_16bit_srgb_after_scaling. One layout is enough — 16-bit layout +// dispatch is covered by the sRGB matrix; this test only discriminates +// linear vs sRGB quantization. +TEST_F(RosImageTextureTestFixture, bayer_16bit_default_is_linear_passthrough) { + const std::string encoding = sensor_msgs::image_encodings::BAYER_RGGB16; + const uint32_t w = 8; + const uint32_t h = 8; + const uint16_t r = 511; + + auto msg = makeImage(w, h, encoding, + buildUniformMosaic(w, h, r, /*g*/ 0, /*b*/ 0, encoding), 2); + + ROSImageTexture texture; // linear_input_ defaults to false + texture.setNormalizeFloatImage(false, 0.0, 1023.0); + texture.addMessage(msg); + ASSERT_TRUE(texture.update()); + + const std::vector rgb = readTextureRGB(texture); + const uint8_t expected_r = static_cast(std::lround(511.0 * 255.0 / 1023.0)); + + // The expected ~127 also discriminates against sRGB mode (~188). + for (uint32_t y = 2; y + 2 < h; ++y) { + for (uint32_t x = 2; x + 2 < w; ++x) { + const size_t i = rgbIndex(y, x, w); + EXPECT_NEAR(rgb[i + 0], expected_r, 1) << "at (" << y << "," << x << ")"; + } + } +} + +// 16-bit Bayer with normalize=false and a fixed max deliberately *larger* +// than the actual peak input: the fixed max must be respected. This +// discriminates the "use fixed bounds" path from an accidental auto-detect +// (which would remap the actual 1023 peak to 255 regardless of the user's +// Max Value setting). +TEST_F(RosImageTextureTestFixture, bayer_16bit_fixed_max_above_actual_is_respected) { + const std::string encoding = sensor_msgs::image_encodings::BAYER_RGGB16; + const uint32_t w = 8; + const uint32_t h = 8; + const uint16_t r = 1023; // actual peak + + auto msg = makeImage(w, h, encoding, + buildUniformMosaic(w, h, r, /*g*/ 0, /*b*/ 0, encoding), 2); + + ROSImageTexture texture; // linear passthrough + texture.setNormalizeFloatImage(false, 0.0, 4095.0); // user says 12-bit range + texture.addMessage(msg); + ASSERT_TRUE(texture.update()); + + const std::vector rgb = readTextureRGB(texture); + // 1023 / 4095 * 255 ~= 63. Auto-detected max (1023) would give 255. + const uint8_t expected_r = static_cast(std::lround(1023.0 * 255.0 / 4095.0)); + + for (uint32_t y = 2; y + 2 < h; ++y) { + for (uint32_t x = 2; x + 2 < w; ++x) { + const size_t i = rgbIndex(y, x, w); + EXPECT_NEAR(rgb[i + 0], expected_r, 1) << "at (" << y << "," << x << ")"; + } + } +} + +// Toggling linear_input at runtime must re-run the converter on the held +// frame — the same latched-topic contract that setSmoothScaling honors. +TEST_F(RosImageTextureTestFixture, toggling_linear_input_reprocesses_held_frame) { + const std::string encoding = sensor_msgs::image_encodings::BAYER_RGGB8; + const uint32_t w = 8; + const uint32_t h = 8; + const uint8_t r = 200; + + auto msg = makeImage(w, h, encoding, + buildUniformMosaic(w, h, r, 0, 0, encoding), 1); + + ROSImageTexture texture; + texture.addMessage(msg); + ASSERT_TRUE(texture.update()); + + // Baseline: linear pass-through (default). + { + const std::vector rgb = readTextureRGB(texture); + const size_t i = rgbIndex(4, 4, w); + EXPECT_EQ(rgb[i + 0], r); + } + + // Toggle on without a new message — next update() must re-encode. + texture.setLinearInput(true); + ASSERT_TRUE(texture.update()); + { + const std::vector rgb = readTextureRGB(texture); + const size_t i = rgbIndex(4, 4, w); + EXPECT_NEAR(rgb[i + 0], srgbEncodeByte(r / 255.0), 2); + } + + // Toggle back off — must re-encode back to the linear value. + texture.setLinearInput(false); + ASSERT_TRUE(texture.update()); + { + const std::vector rgb = readTextureRGB(texture); + const size_t i = rgbIndex(4, 4, w); + EXPECT_EQ(rgb[i + 0], r); + } +} + +// Median-frames path on 16-bit Bayer: with a running window of 5 frames, +// feed four high-peak frames followed by one low-peak frame. Median max is +// then still the high peak, so the low-peak frame is normalised against the +// median (high) rather than its own peak — the low-peak red pixels appear +// dim, not saturated. Behaviour discriminator: with median filtering, the +// interior red is around low_peak * 255 / median (~= 32); without it, red +// would saturate at 255. +TEST_F(RosImageTextureTestFixture, bayer_16bit_median_frames_tracks_median_not_last) { + const std::string encoding = sensor_msgs::image_encodings::BAYER_RGGB16; + const uint32_t w = 8; + const uint32_t h = 8; + const uint16_t high_peak = 4095; + const uint16_t low_peak = 511; + + ROSImageTexture texture; + texture.setNormalizeFloatImage(true); + texture.setMedianFrames(5); + + // Four high frames + one low: sorted maxes = [511, 4095, 4095, 4095, 4095], + // median = 4095. + for (uint16_t peak : {high_peak, high_peak, high_peak, high_peak, low_peak}) { + auto msg = makeImage(w, h, encoding, + buildUniformMosaic(w, h, peak, /*g*/ 0, /*b*/ 0, encoding), 2); + texture.addMessage(msg); + ASSERT_TRUE(texture.update()); + } + + const std::vector rgb = readTextureRGB(texture); + const uint8_t expected_r = + static_cast(std::lround(low_peak * 255.0 / high_peak)); // ~= 32 + ASSERT_LT(expected_r, 60u); // discriminates against no-median (255) + + for (uint32_t y = 2; y + 2 < h; ++y) { + for (uint32_t x = 2; x + 2 < w; ++x) { + const size_t i = rgbIndex(y, x, w); + EXPECT_NEAR(rgb[i + 0], expected_r, 3) << "at (" << y << "," << x << ")"; + } + } +} + +// Small-image smoke tests: 2x2 and 3x3 inputs must not crash or produce NaN. +// These exercise the border-only path and the corner cases of the bilinear +// neighbour averaging (count of valid neighbours < 4). +TEST_F(RosImageTextureTestFixture, small_2x2_image_borders_produce_expected_colour) { + // Pure-red mosaic on the smallest possible image: every output pixel is + // computed by the border handler with fewer than 4 valid neighbours. All + // R samples equal 200; all G and B sensels are 0. Every output pixel + // should therefore show R dominant and near 200, with G and B near 0. + const std::string encoding = sensor_msgs::image_encodings::BAYER_RGGB8; + const uint8_t r = 200; + auto msg = makeImage( + 2, 2, encoding, buildUniformMosaic(2, 2, r, /*g*/ 0, /*b*/ 0, encoding), 1); + + ROSImageTexture texture; + texture.addMessage(msg); + ASSERT_TRUE(texture.update()); + + const std::vector rgb = readTextureRGB(texture); + ASSERT_EQ(rgb.size(), 2u * 2u * 3u); + for (uint32_t y = 0; y < 2; ++y) { + for (uint32_t x = 0; x < 2; ++x) { + const size_t i = rgbIndex(y, x, 2); + EXPECT_GE(rgb[i + 0], 100u) << "R at (" << y << "," << x << ")"; + EXPECT_EQ(rgb[i + 1], 0u) << "G at (" << y << "," << x << ")"; + EXPECT_EQ(rgb[i + 2], 0u) << "B at (" << y << "," << x << ")"; + } + } +} + +TEST_F(RosImageTextureTestFixture, small_3x3_image_borders_produce_expected_colour) { + // Same idea on a 3x3 BGGR: the centre pixel gets the full-neighbourhood + // path; the 8 border pixels exercise the corner / edge branches of the + // handler. All R samples equal 200; G and B are 0; output R should be + // dominant everywhere. + const std::string encoding = sensor_msgs::image_encodings::BAYER_BGGR8; + const uint8_t r = 200; + auto msg = makeImage( + 3, 3, encoding, buildUniformMosaic(3, 3, r, /*g*/ 0, /*b*/ 0, encoding), 1); + + ROSImageTexture texture; + texture.addMessage(msg); + ASSERT_TRUE(texture.update()); + + const std::vector rgb = readTextureRGB(texture); + ASSERT_EQ(rgb.size(), 3u * 3u * 3u); + for (uint32_t y = 0; y < 3; ++y) { + for (uint32_t x = 0; x < 3; ++x) { + const size_t i = rgbIndex(y, x, 3); + EXPECT_GE(rgb[i + 0], 100u) << "R at (" << y << "," << x << ")"; + EXPECT_EQ(rgb[i + 1], 0u) << "G at (" << y << "," << x << ")"; + EXPECT_EQ(rgb[i + 2], 0u) << "B at (" << y << "," << x << ")"; + } + } +} + +// Unknown bayer_* encoding is rejected via UnsupportedImageEncoding. The +// exception propagates out of texture.update(); the display layer catches it +// at image_display.cpp / camera_display.cpp and sets an error status. +TEST_F(RosImageTextureTestFixture, unknown_bayer_encoding_throws_via_update) { + auto msg = makeImage(4, 4, "bayer_xyzzy8", std::vector(16, 0u), 1); + + ROSImageTexture texture; + texture.addMessage(msg); + + EXPECT_THROW(texture.update(), UnsupportedImageEncoding); +} + +// Validation: an oversize message dimension is rejected upfront rather than +// allowed through to the demosaic loops. +TEST_F(RosImageTextureTestFixture, oversize_dimension_rejected) { + auto msg = std::make_shared(); + msg->encoding = sensor_msgs::image_encodings::BAYER_RGGB8; + msg->width = 100000u; + msg->height = 100000u; + msg->step = msg->width; + msg->data = std::vector(16, 0u); // deliberately too small + + ROSImageTexture texture; + texture.addMessage(msg); + EXPECT_THROW(texture.update(), MalformedImageMessage); +} + +// Validation: a truncated data buffer (smaller than height * step) is rejected. +TEST_F(RosImageTextureTestFixture, truncated_data_rejected) { + auto msg = std::make_shared(); + msg->encoding = sensor_msgs::image_encodings::BAYER_RGGB8; + msg->width = 8; + msg->height = 8; + msg->step = 8; + msg->data = std::vector(8, 0u); // 8 bytes but expects 64 + + ROSImageTexture texture; + texture.addMessage(msg); + EXPECT_THROW(texture.update(), MalformedImageMessage); +} + +// Padded-stride 16-bit Bayer: a message where step > width * 2 (extra +// padding bytes at the end of each row, as some drivers emit for SIMD +// alignment) must still demosaic correctly — the buffer is repacked in +// update() before the converter sees it. Padding bytes are 0xFF: if the +// repack ever skipped a row (or was bypassed and padding leaked into the +// min/max scan), max_value would jump to 65535 and the actual red pixels +// (1023) would map to a near-black ~4/255. Full red (>= 250) is therefore +// a strong discriminator that the repack ran end-to-end. +TEST_F(RosImageTextureTestFixture, bayer_16bit_padded_stride) { + const std::string encoding = sensor_msgs::image_encodings::BAYER_RGGB16; + const uint32_t w = 8; + const uint32_t h = 8; + const uint32_t step = 24; // 8 pixels * 2 bytes + 8 bytes of padding per row + + std::vector tight = buildUniformMosaic( + w, h, /*r*/ 1023, /*g*/ 0, /*b*/ 0, encoding); + std::vector padded(static_cast(step) * h, uint8_t{0xFF}); + for (uint32_t y = 0; y < h; ++y) { + std::memcpy( + padded.data() + static_cast(y) * step, + tight.data() + static_cast(y) * w * 2u, + w * 2u); + } + + auto msg = std::make_shared(); + msg->width = w; + msg->height = h; + msg->encoding = encoding; + msg->step = step; + msg->data = std::move(padded); + + ROSImageTexture texture; + texture.setNormalizeFloatImage(true); // auto-normalise; default also true + texture.setMedianFrames(1); // single frame -> running median == raw min/max + texture.addMessage(msg); + ASSERT_TRUE(texture.update()); + + const std::vector rgb = readTextureRGB(texture); + for (uint32_t y = 2; y + 2 < h; ++y) { + for (uint32_t x = 2; x + 2 < w; ++x) { + const size_t i = rgbIndex(y, x, w); + // Red pixels saturate (max_value == 1023 from the actual R samples). + EXPECT_GE(rgb[i + 0], 250u) << "at (" << y << "," << x << ")"; + // G and B sensels are zero, so the demosaiced G/B output is zero + // regardless of the normalisation range. + EXPECT_LE(rgb[i + 1], 5u) << "at (" << y << "," << x << ")"; + EXPECT_LE(rgb[i + 2], 5u) << "at (" << y << "," << x << ")"; + } + } +}