CvSink: Support grayscale images.

Also support 4-channel BGRx images and provide better error reporting
on bad images passed to PutFrame.

include/cscore_c.h

@@ -77,7 +77,8 @@ enum CS_PixelFormat {
   CS_PIXFMT_MJPEG,
   CS_PIXFMT_YUYV,
   CS_PIXFMT_RGB565,
-  CS_PIXFMT_BGR
+  CS_PIXFMT_BGR,
+  CS_PIXFMT_GRAY
 };
 
 //

include/cscore_cpp.h

@@ -49,7 +49,8 @@ struct VideoMode : public CS_VideoMode {
     kMJPEG = CS_PIXFMT_MJPEG,
     kYUYV = CS_PIXFMT_YUYV,
     kRGB565 = CS_PIXFMT_RGB565,
-    kBGR = CS_PIXFMT_BGR
+    kBGR = CS_PIXFMT_BGR,
+    kGray = CS_PIXFMT_GRAY
   };
   VideoMode() {
     pixelFormat = 0;

include/cscore_oo.h

@@ -280,8 +280,9 @@ class CvSource : public VideoSource {
            int height, int fps);
 
   /// Put an OpenCV image and notify sinks.
-  /// This is identical in behavior to calling PutImage(0, image) followed by
-  /// NotifyFrame().
+  /// 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
+  /// cv::Mat::convertTo() and/or cv::cvtColor() to convert it first.
   /// @param image OpenCV image
   void PutFrame(cv::Mat& image);
 
@@ -439,6 +440,7 @@ class CvSink : public VideoSink {
   void SetDescription(llvm::StringRef description);
 
   /// Wait for the next frame and get the image.
+  /// The provided image will have three 8-bit channels stored in BGR order.
   /// @return Frame time, or 0 on error (call GetError() to obtain the error
   ///         message);
   uint64_t GrabFrame(cv::Mat& image) const;

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

@@ -39,6 +39,7 @@ public class CvSink extends VideoSink {
   }
 
   /// Wait for the next frame and get the image.
+  /// The provided image will have three 3-bit channels stored in BGR order.
   /// @return Frame time, or 0 on error (call GetError() to obtain the error
   ///         message);
   public long grabFrame(Mat image) {

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

@@ -29,6 +29,9 @@ public class CvSource extends VideoSource {
   }
 
   /// Put an OpenCV image and notify sinks.
+  /// 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.
   /// @param image OpenCV image
   public void putFrame(Mat image) {
     CameraServerJNI.putSourceFrame(m_handle, image.nativeObj);

java/src/edu/wpi/cscore/VideoMode.java

@@ -10,7 +10,7 @@ package edu.wpi.cscore;
 /// Video mode
 public class VideoMode {
   public enum PixelFormat {
-    kUnknown(0), kMJPEG(1), kYUYV(2), kRGB565(3), kBGR(4);
+    kUnknown(0), kMJPEG(1), kYUYV(2), kRGB565(3), kBGR(4), kGray(5);
     private int value;
 
     private PixelFormat(int value) {

src/CvSourceImpl.cpp

@@ -9,6 +9,7 @@
 
 #include "llvm/STLExtras.h"
 #include "opencv2/core/core.hpp"
+#include "opencv2/imgproc/imgproc.hpp"
 #include "opencv2/highgui/highgui.hpp"
 #include "support/timestamp.h"
 
@@ -95,9 +96,35 @@ void CvSourceImpl::NumSinksEnabledChanged() {
 }
 
 void CvSourceImpl::PutFrame(cv::Mat& image) {
-  auto dest = AllocImage(VideoMode::kBGR, image.cols, image.rows,
-                         image.total() * image.elemSize());
-  image.copyTo(dest->AsMat());
+  // We only support 8-bit images; convert if necessary.
+  cv::Mat finalImage;
+  if (image.depth() == CV_8U)
+    finalImage = image;
+  else
+    image.convertTo(finalImage, CV_8U);
+
+  std::unique_ptr<Image> dest;
+  switch (image.channels()) {
+    case 1:
+      dest =
+          AllocImage(VideoMode::kGray, image.cols, image.rows, image.total());
+      finalImage.copyTo(dest->AsMat());
+      break;
+    case 3:
+      dest = AllocImage(VideoMode::kBGR, image.cols, image.rows,
+                        image.total() * 3);
+      finalImage.copyTo(dest->AsMat());
+      break;
+    case 4:
+      dest = AllocImage(VideoMode::kBGR, image.cols, image.rows,
+                        image.total() * 3);
+      cv::cvtColor(finalImage, dest->AsMat(), cv::COLOR_BGRA2BGR);
+      break;
+    default:
+      SERROR("PutFrame: " << image.channels()
+                          << "-channel images not supported");
+      return;
+  }
   SourceImpl::PutFrame(std::move(dest), wpi::Now());
 }
 

src/Frame.cpp

@@ -163,7 +163,7 @@ Image* Frame::Convert(Image* image, VideoMode::PixelFormat pixelFormat,
   // Color convert; if ultimate destination is JPEG, we need to convert to BGR
   switch (pixelFormat) {
     case VideoMode::kRGB565:
-      // If source is YUYV, need to convert to BGR first
+      // If source is YUYV or Gray, need to convert to BGR first
       if (cur->pixelFormat == VideoMode::kYUYV) {
         // Check to see if BGR version already exists...
         if (Image* newImage =
@@ -171,14 +171,45 @@ Image* Frame::Convert(Image* image, VideoMode::PixelFormat pixelFormat,
           cur = newImage;
         else
           cur = ConvertYUYVToBGR(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);
       }
       return ConvertBGRToRGB565(cur);
+    case VideoMode::kGray:
+      // If source is YUYV or RGB565, need to convert to BGR first
+      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::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);
+      }
+      return ConvertBGRToGray(cur);
     case VideoMode::kBGR:
     case VideoMode::kMJPEG:
       if (cur->pixelFormat == VideoMode::kYUYV)
         cur = ConvertYUYVToBGR(cur);
       else if (cur->pixelFormat == VideoMode::kRGB565)
         cur = ConvertRGB565ToBGR(cur);
+      else if (cur->pixelFormat == VideoMode::kGray) {
+        if (pixelFormat == VideoMode::kBGR)
+          return ConvertGrayToBGR(cur);
+        else
+          return ConvertGrayToMJPEG(cur, jpegQuality);
+      }
       break;
     case VideoMode::kYUYV:
     default:
@@ -213,6 +244,27 @@ Image* Frame::ConvertMJPEGToBGR(Image* image) {
   return rv;
 }
 
+Image* Frame::ConvertMJPEGToGray(Image* image) {
+  if (!image || image->pixelFormat != VideoMode::kMJPEG) return nullptr;
+
+  // Allocate an grayscale image
+  auto newImage =
+      m_impl->source.AllocImage(VideoMode::kGray, image->width, image->height,
+                                image->width * image->height);
+
+  // Decode
+  cv::Mat newMat = newImage->AsMat();
+  cv::imdecode(image->AsInputArray(), cv::IMREAD_GRAYSCALE, &newMat);
+
+  // Save the result
+  Image* rv = newImage.release();
+  if (m_impl) {
+    std::lock_guard<std::recursive_mutex> lock(m_impl->mutex);
+    m_impl->images.push_back(rv);
+  }
+  return rv;
+}
+
 Image* Frame::ConvertYUYVToBGR(Image* image) {
   if (!image || image->pixelFormat != VideoMode::kYUYV) return nullptr;
 
@@ -273,6 +325,46 @@ Image* Frame::ConvertRGB565ToBGR(Image* image) {
   return rv;
 }
 
+Image* Frame::ConvertBGRToGray(Image* image) {
+  if (!image || image->pixelFormat != VideoMode::kBGR) return nullptr;
+
+  // Allocate a Grayscale image
+  auto newImage =
+      m_impl->source.AllocImage(VideoMode::kGray, image->width, image->height,
+                                image->width * image->height);
+
+  // Convert
+  cv::cvtColor(image->AsMat(), newImage->AsMat(), cv::COLOR_BGR2GRAY);
+
+  // Save the result
+  Image* rv = newImage.release();
+  if (m_impl) {
+    std::lock_guard<std::recursive_mutex> lock(m_impl->mutex);
+    m_impl->images.push_back(rv);
+  }
+  return rv;
+}
+
+Image* Frame::ConvertGrayToBGR(Image* image) {
+  if (!image || image->pixelFormat != VideoMode::kBGR) return nullptr;
+
+  // Allocate a BGR image
+  auto newImage =
+      m_impl->source.AllocImage(VideoMode::kBGR, image->width, image->height,
+                                image->width * image->height * 3);
+
+  // Convert
+  cv::cvtColor(image->AsMat(), newImage->AsMat(), cv::COLOR_GRAY2BGR);
+
+  // Save the result
+  Image* rv = newImage.release();
+  if (m_impl) {
+    std::lock_guard<std::recursive_mutex> lock(m_impl->mutex);
+    m_impl->images.push_back(rv);
+  }
+  return rv;
+}
+
 Image* Frame::ConvertBGRToMJPEG(Image* image, int quality) {
   if (!image || image->pixelFormat != VideoMode::kBGR) return nullptr;
   if (!m_impl) return nullptr;
@@ -304,6 +396,37 @@ Image* Frame::ConvertBGRToMJPEG(Image* image, int quality) {
   return rv;
 }
 
+Image* Frame::ConvertGrayToMJPEG(Image* image, int quality) {
+  if (!image || image->pixelFormat != VideoMode::kGray) return nullptr;
+  if (!m_impl) return nullptr;
+  std::lock_guard<std::recursive_mutex> lock(m_impl->mutex);
+
+  // Allocate a JPEG image.  We don't actually know what the resulting size
+  // will be; while the destination will automatically grow, doing so will
+  // cause an extra malloc, so we don't want to be too conservative here.
+  // Per Wikipedia, Q=100 on a sample image results in 8.25 bits per pixel,
+  // this is a little bit more conservative in assuming 25% space savings over
+  // the equivalent grayscale image.
+  auto newImage =
+      m_impl->source.AllocImage(VideoMode::kMJPEG, image->width, image->height,
+                                image->width * image->height * 0.75);
+
+  // Compress
+  if (m_impl->compressionParams.empty()) {
+    m_impl->compressionParams.push_back(CV_IMWRITE_JPEG_QUALITY);
+    m_impl->compressionParams.push_back(quality);
+  } else {
+    m_impl->compressionParams[1] = quality;
+  }
+  cv::imencode(".jpg", image->AsMat(), newImage->vec(),
+               m_impl->compressionParams);
+
+  // Save the result
+  Image* rv = newImage.release();
+  m_impl->images.push_back(rv);
+  return rv;
+}
+
 Image* Frame::GetImage(int width, int height,
                        VideoMode::PixelFormat pixelFormat, int jpegQuality) {
   if (!m_impl) return nullptr;

src/Frame.h

@@ -128,10 +128,14 @@ class Frame {
   Image* Convert(Image* image, VideoMode::PixelFormat pixelFormat,
                  int jpegQuality = 80);
   Image* ConvertMJPEGToBGR(Image* image);
+  Image* ConvertMJPEGToGray(Image* image);
   Image* ConvertYUYVToBGR(Image* image);
   Image* ConvertBGRToRGB565(Image* image);
   Image* ConvertRGB565ToBGR(Image* image);
+  Image* ConvertBGRToGray(Image* image);
+  Image* ConvertGrayToBGR(Image* image);
   Image* ConvertBGRToMJPEG(Image* image, int quality);
+  Image* ConvertGrayToMJPEG(Image* image, int quality);
 
   Image* GetImage(int width, int height, VideoMode::PixelFormat pixelFormat,
                   int jpegQuality = 80);

src/Image.h

@@ -58,6 +58,7 @@ class Image {
       case VideoMode::kBGR:
         type = CV_8UC3;
         break;
+      case VideoMode::kGray:
       case VideoMode::kMJPEG:
       default:
         type = CV_8UC1;

src/UsbCameraImpl.cpp

@@ -67,6 +67,8 @@ static VideoMode::PixelFormat ToPixelFormat(__u32 pixelFormat) {
       return VideoMode::kRGB565;
     case V4L2_PIX_FMT_BGR24:
       return VideoMode::kBGR;
+    case V4L2_PIX_FMT_GREY:
+      return VideoMode::kGray;
     default:
       return VideoMode::kUnknown;
   }
@@ -83,6 +85,8 @@ static __u32 FromPixelFormat(VideoMode::PixelFormat pixelFormat) {
       return V4L2_PIX_FMT_RGB565;
     case VideoMode::kBGR:
       return V4L2_PIX_FMT_BGR24;
+    case VideoMode::kGray:
+      return V4L2_PIX_FMT_GREY;
     default:
       return 0;
   }