diff --git a/compressed_image_transport/CMakeLists.txt b/compressed_image_transport/CMakeLists.txt index 314c4ca0..1c7ef2f1 100644 --- a/compressed_image_transport/CMakeLists.txt +++ b/compressed_image_transport/CMakeLists.txt @@ -28,6 +28,7 @@ add_library( src/compressed_publisher.cpp src/compressed_subscriber.cpp src/manifest.cpp + src/qoi.cpp ) target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES}) diff --git a/compressed_image_transport/cfg/CompressedPublisher.cfg b/compressed_image_transport/cfg/CompressedPublisher.cfg index 856ba598..4fe7a8b6 100755 --- a/compressed_image_transport/cfg/CompressedPublisher.cfg +++ b/compressed_image_transport/cfg/CompressedPublisher.cfg @@ -7,7 +7,8 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() format_enum = gen.enum( [gen.const("jpeg", str_t, "jpeg", "JPEG lossy compression"), - gen.const("png", str_t, "png", "PNG lossless compression")], + gen.const("png", str_t, "png", "PNG lossless compression"), + gen.const("qoi", str_t, "qoi", "QOI lossless compression")], "Enum to set the compression format" ) gen.add("format", str_t, 0, "Compression format", "jpeg", edit_method = format_enum) gen.add("jpeg_quality", int_t, 0, "JPEG quality percentile", 80, 1, 100) diff --git a/compressed_image_transport/compressed_plugins.xml b/compressed_image_transport/compressed_plugins.xml index 5f72db92..e59ec7dd 100644 --- a/compressed_image_transport/compressed_plugins.xml +++ b/compressed_image_transport/compressed_plugins.xml @@ -4,7 +4,7 @@ type="compressed_image_transport::CompressedPublisher" base_class_type="image_transport::PublisherPlugin"> - This plugin publishes a CompressedImage using either JPEG or PNG compression. + This plugin publishes a CompressedImage using either JPEG, PNG or QOI compression. diff --git a/compressed_image_transport/include/compressed_image_transport/compressed_publisher.h b/compressed_image_transport/include/compressed_image_transport/compressed_publisher.h index 32da028d..6b5bed8e 100644 --- a/compressed_image_transport/include/compressed_image_transport/compressed_publisher.h +++ b/compressed_image_transport/include/compressed_image_transport/compressed_publisher.h @@ -66,7 +66,7 @@ class CompressedPublisher : public image_transport::SimplePublisherPlugin +#include +#include +#include +#include +#include + +namespace qoi { + +struct header { + // Image width in pixels + std::uint32_t width{}; + // Image height in pixels + std::uint32_t height{}; + // 3 = RGB, 4 = RGBA + std::uint8_t channels{}; + // 0 = sRGB with linear alpha + // 1 = all channels linear + std::uint8_t colorspace{}; +}; + +constexpr auto SRGB = 0x0; +constexpr auto LINEAR = 0x1; + +constexpr unsigned char OP_RGB = 0b11111110; +constexpr unsigned char OP_RGBA = 0b11111111; +constexpr unsigned char OP_INDEX = 0b0; +constexpr unsigned char OP_DIFF = 0b01000000; +constexpr unsigned char OP_LUMA = 0b10000000; +constexpr unsigned char OP_RUN = 0b11000000; + +constexpr std::array QOI_MAGIC{'q', 'o', 'i', 'f'}; + +constexpr unsigned char END_MARKER_LENGTH = 8; // in bytes +constexpr std::array padding{0, 0, 0, 0, 0, 0, 0, 1}; +constexpr unsigned char HEADER_SIZE = 14; // in bytes + +bool is_valid(std::vector const& bytes) noexcept; + +header get_header(std::vector const& image_to_decode); + +std::vector decode(std::vector const& image_to_decode); + +std::vector encode(std::vector const& orig_pixels, std::uint32_t width, std::uint32_t height, unsigned char channels); + +namespace utils { +std::vector read_binary(std::string const& path); +void write_binary(const std::string& path, std::vector const& bytes); +} // namespace utils + +#ifdef QOI_HEADER_ONLY +#include "qoi.cpp" +#endif + +} // namespace qoi + +#endif diff --git a/compressed_image_transport/package.xml b/compressed_image_transport/package.xml index ca2f1f57..8fb0dc01 100644 --- a/compressed_image_transport/package.xml +++ b/compressed_image_transport/package.xml @@ -11,6 +11,7 @@ http://www.ros.org/wiki/image_transport_plugins Patrick Mihelich Julius Kammerl + Sammy Pfeiffer ament_cmake diff --git a/compressed_image_transport/src/compressed_publisher.cpp b/compressed_image_transport/src/compressed_publisher.cpp index ff9d38e8..9746d0be 100644 --- a/compressed_image_transport/src/compressed_publisher.cpp +++ b/compressed_image_transport/src/compressed_publisher.cpp @@ -39,6 +39,7 @@ #include #include "compressed_image_transport/compression_common.h" +#include "compressed_image_transport/qoi.hpp" #include #include @@ -75,7 +76,7 @@ void CompressedPublisher::advertiseImpl( format_description.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; format_description.description = "Compression method"; format_description.read_only = false; - format_description.additional_constraints = "Supported values: [jpeg, png, tiff]"; + format_description.additional_constraints = "Supported values: [jpeg, png, tiff, qoi]"; try { config_.format = node->declare_parameter(format_param_name, kDefaultFormat, format_description); } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) { @@ -179,7 +180,9 @@ void CompressedPublisher::publish( encodingFormat = PNG; } else if (config_.format == "tiff") { encodingFormat = TIFF; - } + } else if (config_.format == "qoi") { + encodingFormat = QOI; +} // Bit depth of image encoding int bitDepth = enc::bitDepth(message.encoding); @@ -375,6 +378,58 @@ void CompressedPublisher::publish( } break; } + // QOI Compression + case QOI: + { + // Update ros message format header + compressed.format += "; qoi compressed "; + + // Check input format + int channels = message.step / message.width; + if (channels == 3 || channels == 4) + { + + // OpenCV-ros bridge + try + { + cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(message, nullptr, ""); + + // Compress image + const std::size_t size = static_cast(cv_ptr->image.rows) * static_cast(cv_ptr->image.cols) * static_cast(cv_ptr->image.channels()); + std::vector orig_pixels; + orig_pixels.resize(size); + std::memcpy(orig_pixels.data(), cv_ptr->image.data, size); + + compressed.data = qoi::encode(orig_pixels, + cv_ptr->image.cols, + cv_ptr->image.rows, + cv_ptr->image.channels()); + + float cRatio = static_cast((cv_ptr->image.rows * cv_ptr->image.cols * cv_ptr->image.elemSize())) + / static_cast((float)compressed.data.size()); + RCUTILS_LOG_DEBUG("Compressed Image Transport - Codec: qoi, Compression Ratio: 1:%.2f (%lu bytes)", cRatio, compressed.data.size()); + + } + catch (cv_bridge::Exception& e) + { + RCUTILS_LOG_ERROR("%s", e.what()); + return; + } + catch (cv::Exception& e) + { + RCUTILS_LOG_ERROR("%s", e.what()); + return; + } + + // Publish message + publish_fn(compressed); + } else { + RCUTILS_LOG_ERROR( + "Compressed Image Transport - QOI compression requires 3 or 4 channels (input channel number is: %i)", channels); + } + break; + } + default: RCUTILS_LOG_ERROR("Unknown compression type '%s', valid options are 'jpeg', 'png' and 'tiff'", config_.format.c_str()); diff --git a/compressed_image_transport/src/compressed_subscriber.cpp b/compressed_image_transport/src/compressed_subscriber.cpp index 335bda8e..23d0d74c 100644 --- a/compressed_image_transport/src/compressed_subscriber.cpp +++ b/compressed_image_transport/src/compressed_subscriber.cpp @@ -40,6 +40,7 @@ #include #include "compressed_image_transport/compression_common.h" +#include "compressed_image_transport/qoi.hpp" #include @@ -117,64 +118,90 @@ void CompressedSubscriber::internalCallback(const CompressedImage::ConstSharedPt // Decode color/mono image try - { - cv_ptr->image = cv::imdecode(cv::Mat(message->data), config_.imdecode_flag); + { - // Assign image encoding string + // Assign image encoding string and get compression format string const size_t split_pos = message->format.find(';'); - if (split_pos==std::string::npos) - { - // Older version of compressed_image_transport does not signal image format - switch (cv_ptr->image.channels()) - { - case 1: - cv_ptr->encoding = enc::MONO8; - break; - case 3: - cv_ptr->encoding = enc::BGR8; - break; - default: - RCLCPP_ERROR(logger_, "Unsupported number of channels: %i", cv_ptr->image.channels()); - break; - } - } else - { - std::string image_encoding = message->format.substr(0, split_pos); + std::string image_encoding = message->format.substr(0, split_pos); + std::string compression_format = message->format.substr(split_pos+2, 3); - cv_ptr->encoding = image_encoding; + if (compression_format == "qoi") + { + auto header = qoi::get_header(message->data); + auto img_pixels = qoi::decode(message->data); + + // QOI can only do 3 or 4 channels (RGB/RGBA) + cv_ptr->encoding = enc::RGB8; + if (header.channels == 4) + cv_ptr->encoding = enc::RGBA8; + + // I need to make a copy or I get a black image + cv::Mat(header.height, + header.width, + cv_bridge::getCvType(cv_ptr->encoding), + &img_pixels[0]).copyTo(cv_ptr->image); + + // QOI uses RGB, transform to BGR + if (header.channels == 3) + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2BGR); + if (header.channels == 4) + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGBA2BGRA); + } + else{ + cv_ptr->image = cv::imdecode(cv::Mat(message->data), config_.imdecode_flag); - if ( enc::isColor(image_encoding)) + if (split_pos==std::string::npos) { - std::string compressed_encoding = message->format.substr(split_pos); - bool compressed_bgr_image = (compressed_encoding.find("compressed bgr") != std::string::npos); - - // Revert color transformation - if (compressed_bgr_image) + // Older version of compressed_image_transport does not signal image format + switch (cv_ptr->image.channels()) { - // if necessary convert colors from bgr to rgb - if ((image_encoding == enc::RGB8) || (image_encoding == enc::RGB16)) - cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2RGB); - - if ((image_encoding == enc::RGBA8) || (image_encoding == enc::RGBA16)) - cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2RGBA); + case 1: + cv_ptr->encoding = enc::MONO8; + break; + case 3: + cv_ptr->encoding = enc::BGR8; + break; + default: + RCLCPP_ERROR(logger_, "Unsupported number of channels: %i", cv_ptr->image.channels()); + break; + } + } else + { + cv_ptr->encoding = image_encoding; - if ((image_encoding == enc::BGRA8) || (image_encoding == enc::BGRA16)) - cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2BGRA); - } else + if ( enc::isColor(image_encoding)) { - // if necessary convert colors from rgb to bgr - if ((image_encoding == enc::BGR8) || (image_encoding == enc::BGR16)) - cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2BGR); - - if ((image_encoding == enc::BGRA8) || (image_encoding == enc::BGRA16)) - cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2BGRA); - - if ((image_encoding == enc::RGBA8) || (image_encoding == enc::RGBA16)) - cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2RGBA); + std::string compressed_encoding = message->format.substr(split_pos); + bool compressed_bgr_image = (compressed_encoding.find("compressed bgr") != std::string::npos); + + // Revert color transformation + if (compressed_bgr_image) + { + // if necessary convert colors from bgr to rgb + if ((image_encoding == enc::RGB8) || (image_encoding == enc::RGB16)) + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2RGB); + + if ((image_encoding == enc::RGBA8) || (image_encoding == enc::RGBA16)) + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2RGBA); + + if ((image_encoding == enc::BGRA8) || (image_encoding == enc::BGRA16)) + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2BGRA); + } else + { + // if necessary convert colors from rgb to bgr + if ((image_encoding == enc::BGR8) || (image_encoding == enc::BGR16)) + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2BGR); + + if ((image_encoding == enc::BGRA8) || (image_encoding == enc::BGRA16)) + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2BGRA); + + if ((image_encoding == enc::RGBA8) || (image_encoding == enc::RGBA16)) + cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2RGBA); + } + } + if (message->format.find("jpeg") != std::string::npos && enc::bitDepth(image_encoding) == 16) { + cv_ptr->image.convertTo(cv_ptr->image, CV_16U, 256); } - } - if (message->format.find("jpeg") != std::string::npos && enc::bitDepth(image_encoding) == 16) { - cv_ptr->image.convertTo(cv_ptr->image, CV_16U, 256); } } } diff --git a/compressed_image_transport/src/qoi.cpp b/compressed_image_transport/src/qoi.cpp new file mode 100644 index 00000000..8eafcad6 --- /dev/null +++ b/compressed_image_transport/src/qoi.cpp @@ -0,0 +1,267 @@ +/* +From https://github.com/ShadowMitia/libqoi +By Dimitri Belopopsky +MIT License +*/ + +#include "compressed_image_transport/qoi.hpp" + +#include + +namespace qoi { +namespace utils { +std::vector read_binary(std::string const& path) { + std::ifstream file(path, std::fstream::ate | std::fstream::binary); + const auto size = file.tellg(); + file.seekg(0); + std::vector output; + output.resize(static_cast(size)); + if (not file.read(reinterpret_cast(output.data()), size)) { // NOLINT(cppcoreguidelines-pro-type-reinterpret-cast) + std::cerr << "Couldn't read file : " << path << '\n'; + return {}; + } + return output; +} + +void write_binary(const std::string& path, std::vector const& bytes) { + std::ofstream file(path, std::ios::out | std::ios::binary); + const auto size = static_cast(bytes.size()); + if (not file.write(reinterpret_cast(bytes.data()), size)) { // NOLINT(cppcoreguidelines-pro-type-reinterpret-cast) + std::cerr << "Couldn't write file : " << path << '\n'; + } +} + +} // namespace utils +struct RGBA { + unsigned char r{0}; + unsigned char g{0}; + unsigned char b{0}; + unsigned char a{0}; + + friend constexpr bool operator==(RGBA const& a, RGBA const& b) { return a.r == b.r and a.g == b.g and a.b == b.b and a.a == b.a; }; +}; + +std::uint32_t read_4_be_bytes(std::vector const& bytes, std::size_t position) { + const auto res = 0x0U // peekaboo + | std::uint32_t(bytes[position + 0] << 24U) | std::uint32_t(bytes[position + 1] << 16U) | std::uint32_t(bytes[position + 2] << 8U) | + std::uint32_t(bytes[position + 3] << 0U); + return res; +}; + +constexpr std::uint32_t pixel_hash(RGBA const& pix) noexcept { return (pix.r * 3 + pix.g * 5 + pix.b * 7 + pix.a * 11); }; + +bool is_valid(const std::vector& bytes) noexcept { + if (bytes.size() < qoi::HEADER_SIZE) { + return false; + } + + if (bytes[0] != QOI_MAGIC[0] and bytes[1] != QOI_MAGIC[1] and bytes[2] != QOI_MAGIC[3]) { + return false; + } + + return true; +} + +header get_header(std::vector const& image_to_decode) { + header image_header{}; + image_header.width = read_4_be_bytes(image_to_decode, 4); + image_header.height = read_4_be_bytes(image_to_decode, 8); + image_header.channels = image_to_decode[12]; + image_header.colorspace = image_to_decode[13]; + + // fmt::print("Header\n"); + // fmt::print("Magic {:c}{:c}{:c}{:c}\n", image_to_decode[0], image_to_decode[1], image_to_decode[2], image_to_decode[3]); + // fmt::print("Width {}\n", image_header.width); + // fmt::print("Height {}\n", image_header.height); + // fmt::print("Channels {}\n", image_header.channels); + // fmt::print("Colorspace {}\n", image_header.colorspace); + + return image_header; +} + +std::vector decode(std::vector const& image_to_decode) { + + const auto image_header = get_header(image_to_decode); + const auto size = static_cast(image_header.width * image_header.height) * static_cast(image_header.channels); + + std::vector pixels; + pixels.resize(size); + + std::array previous_pixels; + qoi::RGBA previous_pixel{0, 0, 0, 255}; + + std::size_t index = qoi::HEADER_SIZE; + + for (std::size_t pixel_index = 0; pixel_index < size;) { + if (index < (image_to_decode.size() - qoi::END_MARKER_LENGTH)) { + const auto tag = image_to_decode[index++]; + + if (tag == qoi::OP_RGB) { + const auto r = image_to_decode[index++]; + const auto g = image_to_decode[index++]; + const auto b = image_to_decode[index++]; + previous_pixel = qoi::RGBA{r, g, b, previous_pixel.a}; + } else if (tag == qoi::OP_RGBA) { + const auto r = image_to_decode[index++]; + const auto g = image_to_decode[index++]; + const auto b = image_to_decode[index++]; + const auto a = image_to_decode[index++]; + previous_pixel = qoi::RGBA{r, g, b, a}; + } else { + constexpr unsigned char MASK = 0b11000000U; + + if ((tag & MASK) == qoi::OP_INDEX) { + const auto previous_index = (tag & 0b00111111U); + previous_pixel = previous_pixels[previous_index]; + } else if ((tag & MASK) == qoi::OP_DIFF) { + previous_pixel.r += static_cast((((tag & 0b110000U) >> 4U) & 0b11U) - 2); + previous_pixel.g += static_cast((((tag & 0b001100U) >> 2U) & 0b11U) - 2); + previous_pixel.b += static_cast((((tag & 0b000011U) >> 0U) & 0b11U) - 2); + } else if ((tag & MASK) == qoi::OP_LUMA) { + const auto data = image_to_decode[index++]; + const auto vg = static_cast((tag & 0x3FU) - 32); + previous_pixel.r += (vg - 8 + static_cast((data >> 4U) & 0xFU)); + previous_pixel.g += vg; + previous_pixel.b += (vg - 8 + static_cast((data >> 0U) & 0xFU)); + } else if ((tag & MASK) == qoi::OP_RUN) { + const auto run = tag & 0b00111111U; + for (std::size_t j = 0; j < run; j++) { + if (image_header.channels == 4) { + pixels[pixel_index++] = previous_pixel.r; + pixels[pixel_index++] = previous_pixel.g; + pixels[pixel_index++] = previous_pixel.b; + pixels[pixel_index++] = previous_pixel.a; + } else { + pixels[pixel_index++] = previous_pixel.r; + pixels[pixel_index++] = previous_pixel.g; + pixels[pixel_index++] = previous_pixel.b; + } + } + } + } + const auto pixel_hash_index = (pixel_hash(previous_pixel) % 64); + previous_pixels[static_cast(pixel_hash_index)] = previous_pixel; + } + + if (image_header.channels == 4) { + pixels[pixel_index++] = previous_pixel.r; + pixels[pixel_index++] = previous_pixel.g; + pixels[pixel_index++] = previous_pixel.b; + pixels[pixel_index++] = previous_pixel.a; + } else { + pixels[pixel_index++] = previous_pixel.r; + pixels[pixel_index++] = previous_pixel.g; + pixels[pixel_index++] = previous_pixel.b; + } + } + + return pixels; +} + +std::vector encode(std::vector const& orig_pixels, std::uint32_t width, std::uint32_t height, unsigned char channels) { + + // TODO: extract as argument + qoi::header head{}; + head.width = width; + head.height = height; + head.channels = channels; + head.colorspace = qoi::SRGB; + + const auto size = head.width * head.height * head.channels; + const auto qoi_size = head.width * head.height * (head.channels + 1) + qoi::HEADER_SIZE + qoi::END_MARKER_LENGTH; + std::vector encoded_pixels; + encoded_pixels.resize(qoi_size, 0x0); + + std::array previous_pixels; + qoi::RGBA previous_pixel{0, 0, 0, 255}; + qoi::RGBA pixel{0, 0, 0, 255}; + + encoded_pixels[0] = qoi::QOI_MAGIC[0]; + encoded_pixels[1] = qoi::QOI_MAGIC[1]; + encoded_pixels[2] = qoi::QOI_MAGIC[2]; + encoded_pixels[3] = qoi::QOI_MAGIC[3]; + encoded_pixels[4] = static_cast((head.width & 0xFF000000U) >> 24U); + encoded_pixels[5] = static_cast((head.width & 0x00FF0000U) >> 16U); + encoded_pixels[6] = static_cast((head.width & 0x0000FF00U) >> 8U); + encoded_pixels[7] = static_cast((head.width & 0x000000FFU) >> 0U); + encoded_pixels[8] = static_cast((head.height & 0xFF000000U) >> 24U); + encoded_pixels[9] = static_cast((head.height & 0x00FF0000U) >> 16U); + encoded_pixels[10] = static_cast((head.height & 0x0000FF00U) >> 8U); + encoded_pixels[11] = static_cast((head.height & 0x000000FFU) >> 0U); + encoded_pixels[12] = head.channels; + encoded_pixels[13] = head.colorspace; + + std::size_t index = qoi::HEADER_SIZE; + + auto run = 0U; + + for (std::size_t pixel_index = 0; pixel_index < size; pixel_index += head.channels) { + if (head.channels == 4) { + pixel = qoi::RGBA{orig_pixels[pixel_index + 0], orig_pixels[pixel_index + 1], orig_pixels[pixel_index + 2], orig_pixels[pixel_index + 3]}; + } else { + pixel.r = orig_pixels[pixel_index + 0]; + pixel.g = orig_pixels[pixel_index + 1]; + pixel.b = orig_pixels[pixel_index + 2]; + } + if (previous_pixel == pixel) { + run++; + if (run == 62 || pixel_index == (size - head.channels)) { + encoded_pixels[index++] = static_cast(qoi::OP_RUN | (run - 1)); + run = 0; + } + + } else { + + if (run > 0) { + encoded_pixels[index++] = static_cast(qoi::OP_RUN | (run - 1)); + run = 0; + } + + const auto index_pos = static_cast(qoi::pixel_hash(pixel) % 64); + + if (previous_pixels[index_pos] == pixel) { + encoded_pixels[index++] = qoi::OP_INDEX | index_pos; + } else { + previous_pixels[index_pos] = pixel; + + if (pixel.a == previous_pixel.a) { + const auto vr = static_cast(pixel.r - previous_pixel.r); + const auto vg = static_cast(pixel.g - previous_pixel.g); + const auto vb = static_cast(pixel.b - previous_pixel.b); + + const auto vg_r = vr - vg; + const auto vg_b = vb - vg; + + if (vr > -3 and vr < 2 and vg > -3 and vg < 2 and vb > -3 and vb < 2) { + encoded_pixels[index++] = static_cast(qoi::OP_DIFF | (vr + 2) << 4U | (vg + 2) << 2U | (vb + 2)); + } else if (vg_r > -9 and vg_r < 8 and vg > -33 and vg < 32 and vg_b > -9 and vg_b < 8) { + encoded_pixels[index++] = static_cast(qoi::OP_LUMA | static_cast(vg + 32)); + encoded_pixels[index++] = static_cast(static_cast(vg_r + 8) << 4U | static_cast(vg_b + 8)); + } else { + encoded_pixels[index++] = qoi::OP_RGB; + encoded_pixels[index++] = pixel.r; + encoded_pixels[index++] = pixel.g; + encoded_pixels[index++] = pixel.b; + } + } else { + encoded_pixels[index++] = qoi::OP_RGBA; + encoded_pixels[index++] = pixel.r; + encoded_pixels[index++] = pixel.g; + encoded_pixels[index++] = pixel.b; + encoded_pixels[index++] = pixel.a; + } + } + } + previous_pixel = pixel; + } + + for (auto const& pad : padding) { + encoded_pixels[index++] = pad; + } + encoded_pixels.resize(index); + encoded_pixels.shrink_to_fit(); + + return encoded_pixels; +} + +} // namespace qoi