[cscore] Add BGRA support (wpilibsuite#6365)

cscore/src/main/java/edu/wpi/first/cscore/CvSink.java

@@ -36,11 +36,16 @@ private int getCVFormat(PixelFormat pixelFormat) {
     switch (pixelFormat) {
       case kYUYV:
       case kRGB565:
+      case kY16:
+      case kUYVY:
         type = CvType.CV_8UC2;
         break;
       case kBGR:
         type = CvType.CV_8UC3;
         break;
+      case kBGRA:
+        type = CvType.CV_8UC4;
+        break;
       case kGray:
       case kMJPEG:
       default:

cscore/src/main/java/edu/wpi/first/cscore/CvSource.java

@@ -40,16 +40,16 @@ public CvSource(String name, PixelFormat pixelFormat, int width, int height, int
   }
 
   /**
-   * Put an OpenCV image and notify sinks.
+   * Put an OpenCV image and notify sinks
    *
-   * <p>Only 8-bit single-channel or 3-channel (with BGR channel order) images are supported. If the
-   * format, depth or channel order is different, use Mat.convertTo() and/or cvtColor() to convert
-   * it first.
+   * <p>The image format is guessed from the number of channels. The channel mapping is as follows.
+   * 1: kGray 2: kYUYV 3: BGR 4: BGRA Any other channel numbers will throw an error. If your image
+   * is an in alternate format, use the overload that takes a PixelFormat.
    *
-   * @param image OpenCV image
+   * @param image OpenCV Image
    */
   public void putFrame(Mat image) {
-    // We only support 8-bit images, convert if necessary
+    // We only support 8 bit channels
     boolean cleanupRequired = false;
     Mat finalImage;
     if (image.depth() == 0) {
@@ -64,22 +64,64 @@ public void putFrame(Mat image) {
       int channels = finalImage.channels();
       PixelFormat format;
       if (channels == 1) {
+        // 1 channel is assumed Graysacle
         format = PixelFormat.kGray;
+      } else if (channels == 2) {
+        // 2 channels is assumed YUYV
+        format = PixelFormat.kYUYV;
       } else if (channels == 3) {
+        // 3 channels is assumed BGR
         format = PixelFormat.kBGR;
+      } else if (channels == 4) {
+        // 4 channels is assumed BGRA
+        format = PixelFormat.kBGRA;
       } else {
-        throw new VideoException("Unsupported image type");
+        throw new VideoException("Unable to get pixel format for " + channels + " channels");
+      }
+
+      putFrame(finalImage, format, true);
+    } finally {
+      if (cleanupRequired) {
+        finalImage.release();
       }
-      // TODO old code supported BGRA, but the only way I can support that is slow.
-      // Update cscore to support BGRA for raw frames
+    }
+  }
+
+  /**
+   * Put an OpenCV image and notify sinks.
+   *
+   * <p>The format of the Mat must match the PixelFormat. You will corrupt memory if they dont. With
+   * skipVerification false, we will verify the number of channels matches the pixel format. If
+   * skipVerification is true, this step is skipped and is passed straight through.
+   *
+   * @param image OpenCV image
+   * @param format The pixel format of the image
+   * @param skipVerification skip verifying pixel format
+   */
+  public void putFrame(Mat image, PixelFormat format, boolean skipVerification) {
+    // We only support 8-bit images, convert if necessary
+    boolean cleanupRequired = false;
+    Mat finalImage;
+    if (image.depth() == 0) {
+      finalImage = image;
+    } else {
+      finalImage = new Mat();
+      image.convertTo(finalImage, 0);
+      cleanupRequired = true;
+    }
 
+    try {
+      if (!skipVerification) {
+        verifyFormat(finalImage, format);
+      }
+      long step = image.step1() * image.elemSize1();
       CameraServerJNI.putRawSourceFrameData(
           m_handle,
           finalImage.dataAddr(),
-          (int) finalImage.total() * channels,
+          (int) finalImage.total() * finalImage.channels(),
           finalImage.width(),
           finalImage.height(),
-          image.width(),
+          (int) step,
           format.getValue());
 
     } finally {
@@ -88,4 +130,52 @@ public void putFrame(Mat image) {
       }
     }
   }
+
+  private void verifyFormat(Mat image, PixelFormat pixelFormat) {
+    int channels = image.channels();
+    switch (pixelFormat) {
+      case kBGR:
+        if (channels == 3) {
+          return;
+        }
+        break;
+      case kBGRA:
+        if (channels == 4) {
+          return;
+        }
+        break;
+      case kGray:
+        if (channels == 1) {
+          return;
+        }
+        break;
+      case kRGB565:
+        if (channels == 2) {
+          return;
+        }
+        break;
+      case kUYVY:
+        if (channels == 2) {
+          return;
+        }
+        break;
+      case kY16:
+        if (channels == 2) {
+          return;
+        }
+        break;
+      case kYUYV:
+        if (channels == 2) {
+          return;
+        }
+        break;
+      case kMJPEG:
+        if (channels == 1) {
+          return;
+        }
+        break;
+      default:
+        break;
+    }
+  }
 }

cscore/src/main/native/cpp/Frame.cpp

@@ -314,6 +314,54 @@ Image* Frame::ConvertImpl(Image* image, VideoMode::PixelFormat pixelFormat,
         }
       }
       break;
+    case VideoMode::kBGRA:
+      // If source is RGB565, YUYV, UYVY, Gray or Y16, need to convert to BGR
+      // first
+      if (cur->pixelFormat == VideoMode::kRGB565) {
+        // Check to see if BGR version already exists...
+        if (Image* newImage =
+                GetExistingImage(cur->width, cur->height, VideoMode::kBGR)) {
+          cur = newImage;
+        } else {
+          cur = ConvertRGB565ToBGR(cur);
+        }
+      } else if (cur->pixelFormat == VideoMode::kYUYV) {
+        // Check to see if BGR version already exists...
+        if (Image* newImage =
+                GetExistingImage(cur->width, cur->height, VideoMode::kBGR)) {
+          cur = newImage;
+        } else {
+          cur = ConvertYUYVToBGR(cur);
+        }
+      } else if (cur->pixelFormat == VideoMode::kUYVY) {
+        // Check to see if BGR version already exists...
+        if (Image* newImage =
+                GetExistingImage(cur->width, cur->height, VideoMode::kBGR)) {
+          cur = newImage;
+        } else {
+          cur = ConvertUYVYToBGR(cur);
+        }
+      } else if (cur->pixelFormat == VideoMode::kGray) {
+        // Check to see if BGR version already exists...
+        if (Image* newImage =
+                GetExistingImage(cur->width, cur->height, VideoMode::kBGR)) {
+          cur = newImage;
+        } else {
+          cur = ConvertGrayToBGR(cur);
+        }
+      } else if (cur->pixelFormat == VideoMode::kY16) {
+        // Check to see if BGR version already exists...
+        if (Image* newImage =
+                GetExistingImage(cur->width, cur->height, VideoMode::kBGR)) {
+          cur = newImage;
+        } else if (Image* newImage = GetExistingImage(cur->width, cur->height,
+                                                      VideoMode::kGray)) {
+          cur = ConvertGrayToBGR(newImage);
+        } else {
+          cur = ConvertGrayToBGR(ConvertY16ToGray(cur));
+        }
+      }
+      return ConvertBGRToBGRA(cur);
     case VideoMode::kYUYV:
     case VideoMode::kUYVY:
     default:
@@ -664,6 +712,28 @@ Image* Frame::ConvertY16ToGray(Image* image) {
   return rv;
 }
 
+Image* Frame::ConvertBGRToBGRA(Image* image) {
+  if (!image || image->pixelFormat != VideoMode::kBGR) {
+    return nullptr;
+  }
+
+  // Allocate a RGB565 image
+  auto newImage =
+      m_impl->source.AllocImage(VideoMode::kBGRA, image->width, image->height,
+                                image->width * image->height * 4);
+
+  // Convert
+  cv::cvtColor(image->AsMat(), newImage->AsMat(), cv::COLOR_BGR2BGRA);
+
+  // Save the result
+  Image* rv = newImage.release();
+  if (m_impl) {
+    std::scoped_lock lock(m_impl->mutex);
+    m_impl->images.push_back(rv);
+  }
+  return rv;
+}
+
 Image* Frame::GetImageImpl(int width, int height,
                            VideoMode::PixelFormat pixelFormat,
                            int requiredJpegQuality, int defaultJpegQuality) {
@@ -727,3 +797,16 @@ void Frame::ReleaseFrame() {
   m_impl->source.ReleaseFrameImpl(std::unique_ptr<Impl>(m_impl));
   m_impl = nullptr;
 }
+
+namespace cs {
+std::unique_ptr<Image> CreateImageFromBGRA(cs::SourceImpl* source, size_t width,
+                                           size_t height, size_t stride,
+                                           const uint8_t* data) {
+  cv::Mat finalImage{static_cast<int>(height), static_cast<int>(width), CV_8UC4,
+                     const_cast<uint8_t*>(data), stride};
+  std::unique_ptr<Image> dest = source->AllocImage(
+      VideoMode::PixelFormat::kBGR, width, height, width * height * 3);
+  cv::cvtColor(finalImage, dest->AsMat(), cv::COLOR_BGRA2BGR);
+  return dest;
+}
+}  // namespace cs

cscore/src/main/native/cpp/Frame.h

@@ -22,6 +22,10 @@ namespace cs {
 
 class SourceImpl;
 
+std::unique_ptr<Image> CreateImageFromBGRA(cs::SourceImpl* source, size_t width,
+                                           size_t height, size_t stride,
+                                           const uint8_t* data);
+
 class Frame {
   friend class SourceImpl;
 
@@ -206,6 +210,7 @@ class Frame {
   Image* ConvertGrayToMJPEG(Image* image, int quality);
   Image* ConvertGrayToY16(Image* image);
   Image* ConvertY16ToGray(Image* image);
+  Image* ConvertBGRToBGRA(Image* image);
 
   Image* GetImage(int width, int height, VideoMode::PixelFormat pixelFormat) {
     if (pixelFormat == VideoMode::kMJPEG) {

cscore/src/main/native/cpp/Image.h

@@ -63,6 +63,9 @@ class Image {
       case VideoMode::kBGR:
         type = CV_8UC3;
         break;
+      case VideoMode::kBGRA:
+        type = CV_8UC4;
+        break;
       case VideoMode::kGray:
       case VideoMode::kMJPEG:
       default:
@@ -81,6 +84,8 @@ class Image {
         return 2 * width;
       case VideoMode::kBGR:
         return 3 * width;
+      case VideoMode::kBGRA:
+        return 4 * width;
       case VideoMode::kGray:
         return width;
       case VideoMode::kMJPEG:

cscore/src/main/native/cpp/MjpegServerImpl.cpp

@@ -457,6 +457,9 @@ void MjpegServerImpl::ConnThread::SendHTML(wpi::raw_ostream& os,
       case VideoMode::kBGR:
         os << "BGR";
         break;
+      case VideoMode::kBGRA:
+        os << "BGRA";
+        break;
       case VideoMode::kGray:
         os << "gray";
         break;

cscore/src/main/native/cpp/RawSourceImpl.cpp

@@ -22,31 +22,10 @@ RawSourceImpl::RawSourceImpl(std::string_view name, wpi::Logger& logger,
 RawSourceImpl::~RawSourceImpl() = default;
 
 void RawSourceImpl::PutFrame(const WPI_RawFrame& image) {
-  int type;
-  switch (image.pixelFormat) {
-    case VideoMode::kYUYV:
-    case VideoMode::kRGB565:
-    case VideoMode::kY16:
-    case VideoMode::kUYVY:
-      type = CV_8UC2;
-      break;
-    case VideoMode::kBGR:
-      type = CV_8UC3;
-      break;
-    case VideoMode::kGray:
-    case VideoMode::kMJPEG:
-    default:
-      type = CV_8UC1;
-      break;
-  }
-  cv::Mat finalImage{image.height, image.width, type, image.data,
-                     static_cast<size_t>(image.stride)};
-  std::unique_ptr<Image> dest =
-      AllocImage(static_cast<VideoMode::PixelFormat>(image.pixelFormat),
-                 image.width, image.height, image.size);
-  finalImage.copyTo(dest->AsMat());
-
-  SourceImpl::PutFrame(std::move(dest), wpi::Now());
+  auto currentTime = wpi::Now();
+  std::string_view data_view{reinterpret_cast<char*>(image.data), image.size};
+  SourceImpl::PutFrame(static_cast<VideoMode::PixelFormat>(image.pixelFormat),
+                       image.width, image.height, data_view, currentTime);
 }
 
 namespace cs {

cscore/src/main/native/cpp/SourceImpl.cpp

@@ -196,6 +196,8 @@ bool SourceImpl::SetConfigJson(const wpi::json& config, CS_Status* status) {
         mode.pixelFormat = cs::VideoMode::kRGB565;
       } else if (wpi::equals_lower(str, "bgr")) {
         mode.pixelFormat = cs::VideoMode::kBGR;
+      } else if (wpi::equals_lower(str, "bgra")) {
+        mode.pixelFormat = cs::VideoMode::kBGRA;
       } else if (wpi::equals_lower(str, "gray")) {
         mode.pixelFormat = cs::VideoMode::kGray;
       } else if (wpi::equals_lower(str, "y16")) {
@@ -361,6 +363,9 @@ wpi::json SourceImpl::GetConfigJsonObject(CS_Status* status) {
     case VideoMode::kBGR:
       pixelFormat = "bgr";
       break;
+    case VideoMode::kBGRA:
+      pixelFormat = "bgra";
+      break;
     case VideoMode::kGray:
       pixelFormat = "gray";
       break;
@@ -450,6 +455,15 @@ std::unique_ptr<Image> SourceImpl::AllocImage(
 
 void SourceImpl::PutFrame(VideoMode::PixelFormat pixelFormat, int width,
                           int height, std::string_view data, Frame::Time time) {
+  if (pixelFormat == VideoMode::PixelFormat::kBGRA) {
+    // Write BGRA as BGR to save a copy
+    auto image =
+        CreateImageFromBGRA(this, width, height, width * 4,
+                            reinterpret_cast<const uint8_t*>(data.data()));
+    PutFrame(std::move(image), time);
+    return;
+  }
+
   auto image = AllocImage(pixelFormat, width, height, data.size());
 
   // Copy in image data

cscore/src/main/native/cpp/jni/CameraServerJNI.cpp

@@ -12,7 +12,6 @@
 #include <wpi/SmallString.h>
 #include <wpi/jni_util.h>
 
-#include "cscore_cv.h"
 #include "cscore_raw.h"
 #include "cscore_runloop.h"
 #include "edu_wpi_first_cscore_CameraServerJNI.h"