Adding QOI compression support to ROS2

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})

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)

compressed_image_transport/compressed_plugins.xml

@@ -4,7 +4,7 @@
     type="compressed_image_transport::CompressedPublisher"
     base_class_type="image_transport::PublisherPlugin">
     <description>
-      This plugin publishes a CompressedImage using either JPEG or PNG compression.
+      This plugin publishes a CompressedImage using either JPEG, PNG or QOI compression.
     </description>
   </class>
 

compressed_image_transport/include/compressed_image_transport/compressed_publisher.h

@@ -66,7 +66,7 @@ class CompressedPublisher : public image_transport::SimplePublisherPlugin<Compre
                const PublishFn& publish_fn) const;
 
   struct Config {
-    // Compression format to use "jpeg", "png" or "tiff".
+    // Compression format to use "jpeg", "png", "tiff" or "qoi".
     std::string format;
 
     // PNG Compression Level from 0 to 9.  A higher value means a smaller size.

compressed_image_transport/include/compressed_image_transport/compression_common.h

@@ -45,6 +45,7 @@ enum compressionFormat
   JPEG = 0,
   PNG = 1,
   TIFF = 2,
+  QOI = 3,
 };
 
 } //namespace compressed_image_transport

compressed_image_transport/include/compressed_image_transport/qoi.hpp

@@ -0,0 +1,66 @@
+/*
+From https://github.com/ShadowMitia/libqoi
+By Dimitri Belopopsky
+MIT License
+*/
+
+#ifndef QOI_HEADER
+#define QOI_HEADER
+
+#include <array>
+#include <cstddef>
+#include <cstdint>
+#include <cstdlib>
+#include <fstream>
+#include <vector>
+
+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<unsigned char, 4> QOI_MAGIC{'q', 'o', 'i', 'f'};
+
+constexpr unsigned char END_MARKER_LENGTH = 8; // in bytes
+constexpr std::array<unsigned char, 8> padding{0, 0, 0, 0, 0, 0, 0, 1};
+constexpr unsigned char HEADER_SIZE = 14; // in bytes
+
+bool is_valid(std::vector<unsigned char> const& bytes) noexcept;
+
+header get_header(std::vector<unsigned char> const& image_to_decode);
+
+std::vector<unsigned char> decode(std::vector<unsigned char> const& image_to_decode);
+
+std::vector<unsigned char> encode(std::vector<unsigned char> const& orig_pixels, std::uint32_t width, std::uint32_t height, unsigned char channels);
+
+namespace utils {
+std::vector<unsigned char> read_binary(std::string const& path);
+void write_binary(const std::string& path, std::vector<unsigned char> const& bytes);
+} // namespace utils
+
+#ifdef QOI_HEADER_ONLY
+#include "qoi.cpp"
+#endif
+
+} // namespace qoi
+
+#endif

compressed_image_transport/package.xml

@@ -11,6 +11,7 @@
   <url type="website">http://www.ros.org/wiki/image_transport_plugins</url>
   <author>Patrick Mihelich</author>
   <author>Julius Kammerl</author>
+  <author>Sammy Pfeiffer</author>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 

compressed_image_transport/src/compressed_publisher.cpp

@@ -39,6 +39,7 @@
 #include <opencv2/imgcodecs.hpp>
 
 #include "compressed_image_transport/compression_common.h"
+#include "compressed_image_transport/qoi.hpp"
 
 #include <rclcpp/exceptions/exceptions.hpp>
 #include <rclcpp/parameter_client.hpp>
@@ -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<std::size_t>(cv_ptr->image.rows) * static_cast<std::size_t>(cv_ptr->image.cols) * static_cast<std::size_t>(cv_ptr->image.channels());
+          std::vector<unsigned char> 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<float>((cv_ptr->image.rows * cv_ptr->image.cols * cv_ptr->image.elemSize()))
+                / static_cast<float>((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());

compressed_image_transport/src/compressed_subscriber.cpp

@@ -40,6 +40,7 @@
 #include <opencv2/imgproc/imgproc.hpp>
 
 #include "compressed_image_transport/compression_common.h"
+#include "compressed_image_transport/qoi.hpp"
 
 #include <rclcpp/parameter_client.hpp>
 
@@ -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);
       }
     }
   }