Support per-stream resolution settings.

The code now automatically resizes as required.

This change also disconnects camera resolution settings from MJPEG
stream connections; setting the camera resolution can now only be done
via code.

include/cscore_c.h

@@ -76,7 +76,8 @@ enum CS_PixelFormat {
   CS_PIXFMT_UNKNOWN = 0,
   CS_PIXFMT_MJPEG,
   CS_PIXFMT_YUYV,
-  CS_PIXFMT_RGB565
+  CS_PIXFMT_RGB565,
+  CS_PIXFMT_BGR
 };
 
 //

include/cscore_cpp.h

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

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);
+    kUnknown(0), kMJPEG(1), kYUYV(2), kRGB565(3), kBGR(4);
     private int value;
 
     private PixelFormat(int value) {

src/CvSinkImpl.cpp

@@ -44,39 +44,28 @@ void CvSinkImpl::Stop() {
 
 uint64_t CvSinkImpl::GrabFrame(cv::Mat& image) {
   SetEnabled(true);
+
   auto source = GetSource();
   if (!source) {
     // Source disconnected; sleep for one second
     std::this_thread::sleep_for(std::chrono::seconds(1));
     return 0;
   }
+
   auto frame = source->GetNextFrame();  // blocks
   if (!frame) {
     // Bad frame; sleep for 20 ms so we don't consume all processor time.
     std::this_thread::sleep_for(std::chrono::milliseconds(20));
     return 0;  // signal error
   }
-  switch (frame.GetPixelFormat()) {
-    case VideoMode::kMJPEG:
-      cv::imdecode(cv::InputArray{frame.data(), static_cast<int>(frame.size())},
-                   cv::IMREAD_COLOR, &image);
-      // Check to see if we successfully decoded
-      if (image.cols != frame.width() || image.rows != frame.height()) return 0;
-      break;
-    case VideoMode::kYUYV:
-      cv::cvtColor(cv::Mat{frame.height(), frame.width(), CV_8UC2,
-                           frame.data()},
-                   image, cv::COLOR_YUV2BGR_YUYV);
-      break;
-    case VideoMode::kRGB565:
-      cv::cvtColor(cv::Mat{frame.height(), frame.width(), CV_8UC2,
-                           frame.data()},
-                   image, cv::COLOR_BGR5652RGB);
-      break;
-    default:
-      return 0;
+
+  if (!frame.GetCv(image)) {
+    // Shouldn't happen, but just in case...
+    std::this_thread::sleep_for(std::chrono::milliseconds(20));
+    return 0;
   }
-  return frame.time();
+
+  return frame.GetTime();
 }
 
 // Send HTTP response and a stream of JPG-frames

src/CvSourceImpl.cpp

@@ -24,18 +24,11 @@ CvSourceImpl::CvSourceImpl(llvm::StringRef name, const VideoMode& mode)
     : SourceImpl{name} {
   m_mode = mode;
   m_videoModes.push_back(m_mode);
-
-  // Create jpeg quality property
-  m_compressionParams.push_back(CV_IMWRITE_JPEG_QUALITY);
-  m_compressionParams.push_back(80);
 }
 
 CvSourceImpl::~CvSourceImpl() {}
 
-void CvSourceImpl::Start() {
-  m_qualityProperty =
-      CreateProperty("jpeg_quality", CS_PROP_INTEGER, 0, 100, 1, 80, 80);
-}
+void CvSourceImpl::Start() {}
 
 bool CvSourceImpl::CacheProperties(CS_Status* status) const {
   // Doesn't need to do anything.
@@ -93,17 +86,10 @@ void CvSourceImpl::NumSinksEnabledChanged() {
 }
 
 void CvSourceImpl::PutFrame(cv::Mat& image) {
-  std::unique_lock<std::mutex> lock(m_mutex);
-  if (auto prop = GetProperty(m_qualityProperty)) {
-    if (prop->value >= 0 && prop->value <= 100)
-      m_compressionParams[1] = prop->value;
-  }
-  cv::imencode(".jpg", image, m_jpegBuf, m_compressionParams);
-  SourceImpl::PutFrame(
-      VideoMode::kMJPEG, image.cols, image.rows,
-      llvm::StringRef(reinterpret_cast<const char*>(m_jpegBuf.data()),
-                      m_jpegBuf.size()),
-      wpi::Now());
+  auto dest = AllocImage(VideoMode::kBGR, image.cols, image.rows,
+                         image.total() * image.elemSize());
+  image.copyTo(dest->AsMat());
+  SourceImpl::PutFrame(std::move(dest), wpi::Now());
 }
 
 void CvSourceImpl::NotifyError(llvm::StringRef msg) {

src/CvSourceImpl.h

@@ -62,9 +62,6 @@ class CvSourceImpl : public SourceImpl {
 
  private:
   std::atomic_bool m_connected{true};
-  std::vector<unsigned char> m_jpegBuf;
-  std::vector<int> m_compressionParams;
-  int m_qualityProperty;
 };
 
 }  // namespace cs

src/Frame.cpp

@@ -7,11 +7,351 @@
 
 #include "Frame.h"
 
+#include "opencv2/core/core.hpp"
+#include "opencv2/imgproc/imgproc.hpp"
+#include "opencv2/highgui/highgui.hpp"
+
+#include "Log.h"
 #include "SourceImpl.h"
 
 using namespace cs;
 
-void Frame::ReleaseFrame() {
-  m_source->ReleaseFrame(std::unique_ptr<Data>(m_data));
-  m_data = nullptr;
+Frame::Frame(SourceImpl& source, llvm::StringRef error, Time time)
+    : m_impl{source.AllocFrameImpl().release()} {
+  m_impl->refcount = 1;
+  m_impl->error = error;
+  m_impl->time = time;
+}
+
+Frame::Frame(SourceImpl& source, std::unique_ptr<Image> image, Time time)
+    : m_impl{source.AllocFrameImpl().release()} {
+  m_impl->refcount = 1;
+  m_impl->error.resize(0);
+  m_impl->time = time;
+  m_impl->images.push_back(image.release());
+}
+
+Image* Frame::GetNearestImage(int width, int height) const {
+  if (!m_impl) return nullptr;
+  std::lock_guard<std::recursive_mutex> lock(m_impl->mutex);
+  Image* found = nullptr;
+
+  // Ideally we want the smallest image at least width/height in size
+  for (auto i : m_impl->images) {
+    if (i->IsLarger(width, height) && (!found || (i->IsSmaller(*found))))
+      found = i;
+  }
+  if (found) return found;
+
+  // Find the largest image (will be less than width/height)
+  for (auto i : m_impl->images) {
+    if (!found || (i->IsLarger(*found))) found = i;
+  }
+  if (found) return found;
+
+  // Shouldn't reach this, but just in case...
+  return m_impl->images.empty() ? nullptr : m_impl->images[0];
+}
+
+Image* Frame::GetNearestImage(int width, int height,
+                              VideoMode::PixelFormat pixelFormat) const {
+  if (!m_impl) return nullptr;
+  std::lock_guard<std::recursive_mutex> lock(m_impl->mutex);
+  Image* found = nullptr;
+
+  // We want the smallest image at least width/height (or the next largest),
+  // but the primary search order is in order of conversion cost.
+  // If we don't find exactly what we want, we prefer non-JPEG source images
+  // (because JPEG source images require a decompression step).
+  // While the searching takes a little time, it pales in comparison to the
+  // image processing to come, so it's worth spending a little extra time
+  // looking for the most efficient conversion.
+
+  // 1) Same width, height, and pixelFormat (e.g. exactly what we want)
+  for (auto i : m_impl->images) {
+    if (i->Is(width, height, pixelFormat)) return i;
+  }
+
+  // 2) Same width, height, different (but non-JPEG) pixelFormat (color conv)
+  // 2a) If we want JPEG output, prefer BGR over other pixel formats
+  if (pixelFormat == VideoMode::kMJPEG) {
+    for (auto i : m_impl->images) {
+      if (i->Is(width, height, VideoMode::kBGR)) return i;
+    }
+  }
+
+  for (auto i : m_impl->images) {
+    if (i->Is(width, height) && i->pixelFormat != VideoMode::kMJPEG) return i;
+  }
+
+  // 3) Different width, height, same pixelFormat (only if non-JPEG) (resample)
+  if (pixelFormat != VideoMode::kMJPEG) {
+    // 3a) Smallest image at least width/height in size
+    for (auto i : m_impl->images) {
+      if (i->IsLarger(width, height) && i->pixelFormat == pixelFormat &&
+          (!found || (i->IsSmaller(*found))))
+        found = i;
+    }
+    if (found) return found;
+
+    // 3b) Largest image (less than width/height)
+    for (auto i : m_impl->images) {
+      if (i->pixelFormat == pixelFormat && (!found || (i->IsLarger(*found))))
+        found = i;
+    }
+    if (found) return found;
+  }
+
+  // 4) Different width, height, different (but non-JPEG) pixelFormat
+  //    (color conversion + resample)
+  // 4a) Smallest image at least width/height in size
+  for (auto i : m_impl->images) {
+    if (i->IsLarger(width, height) && i->pixelFormat != VideoMode::kMJPEG &&
+        (!found || (i->IsSmaller(*found))))
+      found = i;
+  }
+  if (found) return found;
+
+  // 4b) Largest image (less than width/height)
+  for (auto i : m_impl->images) {
+    if (i->pixelFormat != VideoMode::kMJPEG &&
+        (!found || (i->IsLarger(*found))))
+      found = i;
+  }
+  if (found) return found;
+
+  // 5) Same width, height, JPEG pixelFormat (decompression)
+  for (auto i : m_impl->images) {
+    if (i->Is(width, height, VideoMode::kMJPEG)) return i;
+  }
+
+  // 6) Different width, height, JPEG pixelFormat (decompression)
+  // 6a) Smallest image at least width/height in size
+  for (auto i : m_impl->images) {
+    if (i->IsLarger(width, height) && i->pixelFormat == VideoMode::kMJPEG &&
+        (!found || (i->IsSmaller(*found))))
+      found = i;
+  }
+  if (found) return found;
+
+  // 6b) Largest image (less than width/height)
+  for (auto i : m_impl->images) {
+    if (i->pixelFormat != VideoMode::kMJPEG &&
+        (!found || (i->IsLarger(*found))))
+      found = i;
+  }
+  if (found) return found;
+
+  // Shouldn't reach this, but just in case...
+  return m_impl->images.empty() ? nullptr : m_impl->images[0];
+}
+
+Image* Frame::Convert(Image* image, VideoMode::PixelFormat pixelFormat,
+                      int jpegQuality) {
+  if (!image || image->pixelFormat == pixelFormat) return image;
+  Image* cur = image;
+
+  // If the source image is a JPEG, we need to decode it before we can do
+  // anything else with it.  Note that if the destination format is JPEG, we
+  // still need to do this (unless it was already a JPEG, in which case we
+  // would have returned above).
+  if (cur->pixelFormat == VideoMode::kMJPEG) {
+    cur = ConvertMJPEGToBGR(cur);
+    if (pixelFormat == VideoMode::kBGR) return cur;
+  }
+
+  // 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 (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);
+      }
+      return ConvertBGRToRGB565(cur);
+    case VideoMode::kBGR:
+    case VideoMode::kMJPEG:
+      if (cur->pixelFormat == VideoMode::kYUYV)
+        cur = ConvertYUYVToBGR(cur);
+      else if (cur->pixelFormat == VideoMode::kRGB565)
+        cur = ConvertRGB565ToBGR(cur);
+      break;
+    case VideoMode::kYUYV:
+    default:
+      return nullptr;  // Unsupported
+  }
+
+  // Compress if destination is JPEG
+  if (pixelFormat == VideoMode::kMJPEG)
+    cur = ConvertBGRToMJPEG(cur, jpegQuality);
+
+  return cur;
+}
+
+Image* Frame::ConvertMJPEGToBGR(Image* image) {
+  if (!image || image->pixelFormat != VideoMode::kMJPEG) return nullptr;
+
+  // Allocate an BGR image
+  auto newImage =
+      m_impl->source.AllocImage(VideoMode::kBGR, image->width, image->height,
+                                image->width * image->height * 3);
+
+  // Decode
+  cv::Mat newMat = newImage->AsMat();
+  cv::imdecode(image->AsInputArray(), cv::IMREAD_COLOR, &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;
+
+  // 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_YUV2BGR_YUYV);
+
+  // 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::ConvertBGRToRGB565(Image* image) {
+  if (!image || image->pixelFormat != VideoMode::kBGR) return nullptr;
+
+  // Allocate a RGB565 image
+  auto newImage =
+      m_impl->source.AllocImage(VideoMode::kRGB565, image->width, image->height,
+                                image->width * image->height * 2);
+
+  // Convert
+  cv::cvtColor(image->AsMat(), newImage->AsMat(), cv::COLOR_RGB2BGR565);
+
+  // 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::ConvertRGB565ToBGR(Image* image) {
+  if (!image || image->pixelFormat != VideoMode::kRGB565) 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_BGR5652RGB);
+
+  // 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;
+  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 50% space savings over
+  // the equivalent BGR image.
+  auto newImage =
+      m_impl->source.AllocImage(VideoMode::kMJPEG, image->width, image->height,
+                                image->width * image->height * 1.5);
+
+  // 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;
+  std::lock_guard<std::recursive_mutex> lock(m_impl->mutex);
+  Image* cur = GetNearestImage(width, height, pixelFormat);
+  if (!cur || cur->Is(width, height, pixelFormat)) return cur;
+
+  DEBUG4("converting image from "
+         << cur->width << "x" << cur->height << " type " << cur->pixelFormat
+         << " to " << width << "x" << height << " type " << pixelFormat);
+
+  // If the source image is a JPEG, we need to decode it before we can do
+  // anything else with it.  Note that if the destination format is JPEG, we
+  // still need to do this (unless the width/height were the same, in which
+  // case we already returned the existing JPEG above).
+  if (cur->pixelFormat == VideoMode::kMJPEG) cur = ConvertMJPEGToBGR(cur);
+
+  // Resize
+  if (!cur->Is(width, height)) {
+    // Allocate an image.
+    auto newImage = m_impl->source.AllocImage(
+        cur->pixelFormat, width, height,
+        width * height * (cur->size() / (cur->width * cur->height)));
+
+    // Resize
+    cv::Mat newMat = newImage->AsMat();
+    cv::resize(cur->AsMat(), newMat, newMat.size(), 0, 0);
+
+    // Save the result
+    cur = newImage.release();
+    m_impl->images.push_back(cur);
+  }
+
+  // Convert to output format
+  return Convert(cur, pixelFormat, jpegQuality);
+}
+
+bool Frame::GetCv(cv::Mat& image, int width, int height) {
+  Image* rawImage = GetImage(width, height, VideoMode::kBGR);
+  if (!rawImage) return false;
+  rawImage->AsMat().copyTo(image);
+  return true;
+}
+
+void Frame::ReleaseFrame() {
+  for (auto image : m_impl->images)
+    m_impl->source.ReleaseImage(std::unique_ptr<Image>(image));
+  m_impl->images.clear();
+  m_impl->source.ReleaseFrameImpl(std::unique_ptr<Impl>(m_impl));
+  m_impl = nullptr;
 }

src/Frame.h

@@ -10,10 +10,12 @@
 
 #include <atomic>
 #include <memory>
+#include <mutex>
 
-#include "llvm/StringRef.h"
+#include "llvm/SmallVector.h"
 
 #include "cscore_cpp.h"
+#include "Image.h"
 
 namespace cs {
 
@@ -25,33 +27,28 @@ class Frame {
  public:
   typedef uint64_t Time;
 
-  struct Data {
-    explicit Data(std::size_t capacity_)
-        : data(new char[capacity_]), size(0), capacity(capacity_) {}
-    ~Data() { delete[] data; }
+ private:
+  struct Impl {
+    Impl(SourceImpl& source_) : source(source_) {}
 
+    std::recursive_mutex mutex;
     std::atomic_int refcount{0};
-    Time time;
-    char* data;
-    std::size_t size;
-    std::size_t capacity;
-    VideoMode::PixelFormat pixelFormat;
-    int width;
-    int height;
+    Time time{0};
+    SourceImpl& source;
+    std::string error;
+    llvm::SmallVector<Image*, 4> images;
+    std::vector<int> compressionParams;
   };
 
  public:
-  Frame() noexcept : m_source{nullptr}, m_data{nullptr} {}
+  Frame() noexcept : m_impl{nullptr} {}
 
-  Frame(SourceImpl& source, std::unique_ptr<Data> data) noexcept
-      : m_source{&source},
-        m_data{data.release()} {
-    if (m_data) ++(m_data->refcount);
-  }
+  Frame(SourceImpl& source, llvm::StringRef error, Time time);
 
-  Frame(const Frame& frame) noexcept : m_source{frame.m_source},
-                                       m_data{frame.m_data} {
-    if (m_data) ++(m_data->refcount);
+  Frame(SourceImpl& source, std::unique_ptr<Image> image, Time time);
+
+  Frame(const Frame& frame) noexcept : m_impl{frame.m_impl} {
+    if (m_impl) ++m_impl->refcount;
   }
 
   Frame(Frame&& other) noexcept : Frame() { swap(*this, other); }
@@ -63,64 +60,94 @@ class Frame {
     return *this;
   }
 
-  explicit operator bool() const {
-    return m_data && m_data->pixelFormat != VideoMode::kUnknown;
-  }
-
-  operator llvm::StringRef() const {
-    if (!m_data) return llvm::StringRef{};
-    return llvm::StringRef(m_data->data, m_data->size);
-  }
-
-  std::size_t size() const {
-    if (!m_data) return 0;
-    return m_data->size;
-  }
-
-  const char* data() const {
-    if (!m_data) return nullptr;
-    return m_data->data;
-  }
-
-  char* data() {
-    if (!m_data) return nullptr;
-    return m_data->data;
-  }
-
-  VideoMode::PixelFormat GetPixelFormat() const {
-    if (!m_data) return VideoMode::kUnknown;
-    return m_data->pixelFormat;
-  }
-
-  int width() const {
-    if (!m_data) return 0;
-    return m_data->width;
-  }
-
-  int height() const {
-    if (!m_data) return 0;
-    return m_data->height;
-  }
-
-  Time time() const {
-    if (!m_data) return Time{};
-    return m_data->time;
-  }
+  explicit operator bool() const { return m_impl && m_impl->error.empty(); }
 
   friend void swap(Frame& first, Frame& second) noexcept {
     using std::swap;
-    swap(first.m_source, second.m_source);
-    swap(first.m_data, second.m_data);
+    swap(first.m_impl, second.m_impl);
   }
 
+  Time GetTime() const { return m_impl ? m_impl->time : 0; }
+
+  llvm::StringRef GetError() const {
+    if (!m_impl) return llvm::StringRef{};
+    return m_impl->error;
+  }
+
+  int GetOriginalWidth() const {
+    if (!m_impl) return 0;
+    std::lock_guard<std::recursive_mutex> lock(m_impl->mutex);
+    if (m_impl->images.empty()) return 0;
+    return m_impl->images[0]->width;
+  }
+
+  int GetOriginalHeight() const {
+    if (!m_impl) return 0;
+    std::lock_guard<std::recursive_mutex> lock(m_impl->mutex);
+    if (m_impl->images.empty()) return 0;
+    return m_impl->images[0]->height;
+  }
+
+  int GetOriginalPixelFormat() const {
+    if (!m_impl) return 0;
+    std::lock_guard<std::recursive_mutex> lock(m_impl->mutex);
+    if (m_impl->images.empty()) return 0;
+    return m_impl->images[0]->pixelFormat;
+  }
+
+  Image* GetExistingImage(std::size_t i = 0) const {
+    if (!m_impl) return nullptr;
+    std::lock_guard<std::recursive_mutex> lock(m_impl->mutex);
+    if (i >= m_impl->images.size()) return nullptr;
+    return m_impl->images[i];
+  }
+
+  Image* GetExistingImage(int width, int height) const {
+    if (!m_impl) return nullptr;
+    std::lock_guard<std::recursive_mutex> lock(m_impl->mutex);
+    for (auto i : m_impl->images) {
+      if (i->Is(width, height)) return i;
+    }
+    return nullptr;
+  }
+
+  Image* GetExistingImage(int width, int height,
+                          VideoMode::PixelFormat pixelFormat) const {
+    if (!m_impl) return nullptr;
+    std::lock_guard<std::recursive_mutex> lock(m_impl->mutex);
+    for (auto i : m_impl->images) {
+      if (i->Is(width, height, pixelFormat)) return i;
+    }
+    return nullptr;
+  }
+
+  Image* GetNearestImage(int width, int height) const;
+  Image* GetNearestImage(int width, int height,
+                         VideoMode::PixelFormat pixelFormat) const;
+
+  Image* Convert(Image* image, VideoMode::PixelFormat pixelFormat,
+                 int jpegQuality = 80);
+  Image* ConvertMJPEGToBGR(Image* image);
+  Image* ConvertYUYVToBGR(Image* image);
+  Image* ConvertBGRToRGB565(Image* image);
+  Image* ConvertRGB565ToBGR(Image* image);
+  Image* ConvertBGRToMJPEG(Image* image, int quality);
+
+  Image* GetImage(int width, int height, VideoMode::PixelFormat pixelFormat,
+                  int jpegQuality = 80);
+
+  bool GetCv(cv::Mat& image) {
+    return GetCv(image, GetOriginalWidth(), GetOriginalHeight());
+  }
+  bool GetCv(cv::Mat& image, int width, int height);
+
  private:
   void DecRef() {
-    if (m_data && --(m_data->refcount) == 0) ReleaseFrame();
+    if (m_impl && --(m_impl->refcount) == 0) ReleaseFrame();
   }
   void ReleaseFrame();
 
-  SourceImpl* m_source;
-  Data* m_data;
+  Impl* m_impl;
 };
 
 }  // namespace cs

src/Image.h

@@ -0,0 +1,97 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#ifndef CS_IMAGE_H_
+#define CS_IMAGE_H_
+
+#include <vector>
+
+#include "llvm/StringRef.h"
+
+#include "opencv2/core/core.hpp"
+
+#include "cscore_cpp.h"
+#include "default_init_allocator.h"
+
+namespace cs {
+
+class Frame;
+
+class Image {
+  friend class Frame;
+
+ public:
+  explicit Image(std::size_t capacity)
+      : m_data{capacity, default_init_allocator<char>{}} {
+    m_data.resize(0);
+  }
+  Image(const Image&) = delete;
+  Image& operator=(const Image&) = delete;
+
+  // Getters
+  operator llvm::StringRef() const { return str(); }
+  llvm::StringRef str() const { return llvm::StringRef(data(), size()); }
+  std::size_t capacity() const { return m_data.capacity(); }
+  const char* data() const {
+    return reinterpret_cast<const char*>(m_data.data());
+  }
+  char* data() { return reinterpret_cast<char*>(m_data.data()); }
+  std::size_t size() const { return m_data.size(); }
+
+  const std::vector<uchar>& vec() const { return m_data; }
+  std::vector<uchar>& vec() { return m_data; }
+
+  void resize(std::size_t size) { m_data.resize(size); }
+  void SetSize(std::size_t size) { m_data.resize(size); }
+
+  cv::Mat AsMat() {
+    int type;
+    switch (pixelFormat) {
+      case VideoMode::kYUYV:
+      case VideoMode::kRGB565:
+        type = CV_8UC2;
+        break;
+      case VideoMode::kBGR:
+        type = CV_8UC3;
+        break;
+      case VideoMode::kMJPEG:
+      default:
+        type = CV_8UC1;
+        break;
+    }
+    return cv::Mat{height, width, type, m_data.data()};
+  }
+
+  cv::_InputArray AsInputArray() { return cv::_InputArray{m_data}; }
+
+  bool Is(int width_, int height_) {
+    return width == width_ && height == height_;
+  }
+  bool Is(int width_, int height_, VideoMode::PixelFormat pixelFormat_) {
+    return width == width_ && height == height_ && pixelFormat == pixelFormat_;
+  }
+  bool IsLarger(int width_, int height_) {
+    return width >= width_ && height >= height_;
+  }
+  bool IsLarger(const Image& oth) {
+    return width >= oth.width && height >= oth.height;
+  }
+  bool IsSmaller(int width_, int height_) { return !IsLarger(width_, height_); }
+  bool IsSmaller(const Image& oth) { return !IsLarger(oth); }
+
+ private:
+  std::vector<uchar> m_data;
+
+ public:
+  VideoMode::PixelFormat pixelFormat{VideoMode::kUnknown};
+  int width{0};
+  int height{0};
+};
+
+}  // namespace cs
+
+#endif  // CS_IMAGE_H_

src/MjpegServerImpl.cpp

@@ -73,6 +73,11 @@ class MjpegServerImpl::ConnThread : public wpi::SafeThread {
     m_source->DisableSink();
     m_streaming = false;
   }
+
+  int m_width{0};
+  int m_height{0};
+  int m_compression{80};
+  int m_fps{0};
 };
 
 // Standard header to send along with other header information like mimetype.
@@ -177,31 +182,27 @@ bool MjpegServerImpl::ConnThread::ProcessCommand(llvm::raw_ostream& os,
       return false;
     }
 
-    // handle resolution and FPS; these are handled via separate interfaces
-    // rather than as properties
+    // Handle resolution, compression, and FPS.  These are handled locally
+    // rather than passed to the source.
     if (param == "resolution") {
       llvm::StringRef widthStr, heightStr;
       std::tie(widthStr, heightStr) = value.split('x');
       int width, height;
       if (widthStr.getAsInteger(10, width)) {
-        response << param << ": \"width is not integer\"\r\n";
+        response << param << ": \"width is not an integer\"\r\n";
         SWARNING("HTTP parameter \"" << param << "\" width \"" << widthStr
-                                     << "\" is not integer");
+                                     << "\" is not an integer");
         continue;
       }
       if (heightStr.getAsInteger(10, height)) {
-        response << param << ": \"height is not integer\"\r\n";
+        response << param << ": \"height is not an integer\"\r\n";
         SWARNING("HTTP parameter \"" << param << "\" height \"" << heightStr
-                                     << "\" is not integer");
+                                     << "\" is not an integer");
         continue;
       }
-      CS_Status status = 0;
-      if (!source.SetResolution(width, height, &status)) {
-        response << param << ": \"error\"\r\n";
-        SWARNING("Could not set resolution to " << width << "x" << height);
-      } else {
-        response << param << ": \"ok\"\r\n";
-      }
+      m_width = width;
+      m_height = height;
+      response << param << ": \"ok\"\r\n";
       continue;
     }
 
@@ -212,12 +213,22 @@ bool MjpegServerImpl::ConnThread::ProcessCommand(llvm::raw_ostream& os,
         SWARNING("HTTP parameter \"" << param << "\" value \"" << value
                                      << "\" is not an integer");
         continue;
-      }
-      CS_Status status = 0;
-      if (!source.SetFPS(fps, &status)) {
-        response << param << ": \"error\"\r\n";
-        SWARNING("Could not set FPS to " << fps);
       } else {
+        m_fps = fps;
+        response << param << ": \"ok\"\r\n";
+      }
+      continue;
+    }
+
+    if (param == "compression") {
+      int compression;
+      if (value.getAsInteger(10, compression)) {
+        response << param << ": \"invalid integer\"\r\n";
+        SWARNING("HTTP parameter \"" << param << "\" value \"" << value
+                                     << "\" is not an integer");
+        continue;
+      } else {
+        m_compression = compression;
         response << param << ": \"ok\"\r\n";
       }
       continue;
@@ -229,7 +240,6 @@ bool MjpegServerImpl::ConnThread::ProcessCommand(llvm::raw_ostream& os,
     // try to assign parameter
     auto prop = source.GetPropertyIndex(param);
     if (!prop) {
-      if (param == "compression") continue;  // silently ignore
       response << param << ": \"ignored\"\r\n";
       SWARNING("ignoring HTTP parameter \"" << param << "\"");
       continue;
@@ -411,11 +421,21 @@ void MjpegServerImpl::ConnThread::SendStream(wpi::raw_socket_ostream& os) {
       continue;
     }
 
-    const char* data = frame.data();
-    std::size_t size = frame.size();
+    int width = m_width != 0 ? m_width : frame.GetOriginalWidth();
+    int height = m_height != 0 ? m_height : frame.GetOriginalHeight();
+    Image* image =
+        frame.GetImage(width, height, VideoMode::kMJPEG, m_compression);
+    if (!image) {
+      // Shouldn't happen, but just in case...
+      std::this_thread::sleep_for(std::chrono::milliseconds(20));
+      continue;
+    }
+
+    const char* data = image->data();
+    std::size_t size = image->size();
     bool addDHT = false;
     std::size_t locSOF = size;
-    switch (frame.GetPixelFormat()) {
+    switch (image->pixelFormat) {
       case VideoMode::kMJPEG:
         // Determine if we need to add DHT to it, and allocate enough space
         // for adding it if required.
@@ -434,7 +454,7 @@ void MjpegServerImpl::ConnThread::SendStream(wpi::raw_socket_ostream& os) {
     // print the individual mimetype and the length
     // sending the content-length fixes random stream disruption observed
     // with firefox
-    double timestamp = frame.time() / 10000000.0;
+    double timestamp = frame.GetTime() / 10000000.0;
     header.clear();
     oss << "\r\n--" BOUNDARY "\r\n"
         << "Content-Type: image/jpeg\r\n"
@@ -446,7 +466,7 @@ void MjpegServerImpl::ConnThread::SendStream(wpi::raw_socket_ostream& os) {
       // Insert DHT data immediately before SOF
       os << llvm::StringRef(data, locSOF);
       os << JpegGetDHT();
-      os << llvm::StringRef(data + locSOF, frame.size() - locSOF);
+      os << llvm::StringRef(data + locSOF, image->size() - locSOF);
     } else {
       os << llvm::StringRef(data, size);
     }
@@ -459,6 +479,12 @@ void MjpegServerImpl::ConnThread::ProcessRequest() {
   wpi::raw_socket_istream is{*m_stream};
   wpi::raw_socket_ostream os{*m_stream, true};
 
+  // Reset per-request settings
+  m_width = 0;
+  m_height = 0;
+  m_compression = 80;
+  m_fps = 0;
+
   // Read the request string from the stream
   bool error = false;
   llvm::SmallString<128> reqBuf;

src/SinkImpl.cpp

@@ -81,17 +81,16 @@ void SinkImpl::SetSource(std::shared_ptr<SourceImpl> source) {
 std::string SinkImpl::GetError() const {
   std::lock_guard<std::mutex> lock(m_mutex);
   if (!m_source) return "no source connected";
-  auto frame = m_source->GetCurFrame();
-  if (frame) return std::string{};  // no error
-  return llvm::StringRef{frame};
+  return m_source->GetCurFrame().GetError();
 }
 
 llvm::StringRef SinkImpl::GetError(llvm::SmallVectorImpl<char>& buf) const {
   std::lock_guard<std::mutex> lock(m_mutex);
   if (!m_source) return "no source connected";
-  auto frame = m_source->GetCurFrame();
-  if (frame) return llvm::StringRef{};  // no error
-  buf.append(frame.data(), frame.data() + frame.size());
+  // Make a copy as it's shared data
+  llvm::StringRef error = m_source->GetCurFrame().GetError();
+  buf.clear();
+  buf.append(error.data(), error.data() + error.size());
   return llvm::StringRef{buf.data(), buf.size()};
 }
 

src/SourceImpl.cpp

@@ -10,15 +10,17 @@
 #include <algorithm>
 #include <cstring>
 
+#include "llvm/STLExtras.h"
+
 #include "Log.h"
 #include "Notifier.h"
 
 using namespace cs;
 
-static constexpr std::size_t kMaxFramesAvail = 32;
+static constexpr std::size_t kMaxImagesAvail = 32;
 
 SourceImpl::SourceImpl(llvm::StringRef name)
-    : m_name{name}, m_frame{*this, nullptr} {}
+    : m_name{name}, m_frame{*this, llvm::StringRef{}, 0} {}
 
 SourceImpl::~SourceImpl() {
   // Wake up anyone who is waiting.  This also clears the current frame,
@@ -55,7 +57,7 @@ void SourceImpl::SetConnected(bool connected) {
 
 uint64_t SourceImpl::GetCurFrameTime() {
   std::unique_lock<std::mutex> lock{m_frameMutex};
-  return m_frame.time();
+  return m_frame.GetTime();
 }
 
 Frame SourceImpl::GetCurFrame() {
@@ -65,15 +67,15 @@ Frame SourceImpl::GetCurFrame() {
 
 Frame SourceImpl::GetNextFrame() {
   std::unique_lock<std::mutex> lock{m_frameMutex};
-  auto oldTime = m_frame.time();
-  m_frameCv.wait(lock, [=] { return m_frame.time() != oldTime; });
+  auto oldTime = m_frame.GetTime();
+  m_frameCv.wait(lock, [=] { return m_frame.GetTime() != oldTime; });
   return m_frame;
 }
 
 void SourceImpl::Wakeup() {
   {
     std::lock_guard<std::mutex> lock{m_frameMutex};
-    m_frame = Frame{*this, nullptr};
+    m_frame = Frame{*this, llvm::StringRef{}, 0};
   }
   m_frameCv.notify_all();
 }
@@ -261,20 +263,20 @@ std::vector<VideoMode> SourceImpl::EnumerateVideoModes(
   return m_videoModes;
 }
 
-std::unique_ptr<Frame::Data> SourceImpl::AllocFrame(
-    VideoMode::PixelFormat pixelFormat, int width, int height, std::size_t size,
-    Frame::Time time) {
-  std::unique_ptr<Frame::Data> frameData;
+std::unique_ptr<Image> SourceImpl::AllocImage(
+    VideoMode::PixelFormat pixelFormat, int width, int height,
+    std::size_t size) {
+  std::unique_ptr<Image> image;
   {
     std::lock_guard<std::mutex> lock{m_poolMutex};
     // find the smallest existing frame that is at least big enough.
     int found = -1;
-    for (std::size_t i = 0; i < m_framesAvail.size(); ++i) {
+    for (std::size_t i = 0; i < m_imagesAvail.size(); ++i) {
       // is it big enough?
-      if (m_framesAvail[i] && m_framesAvail[i]->capacity >= size) {
+      if (m_imagesAvail[i] && m_imagesAvail[i]->capacity() >= size) {
         // is it smaller than the last found?
         if (found < 0 ||
-            m_framesAvail[i]->capacity < m_framesAvail[found]->capacity) {
+            m_imagesAvail[i]->capacity() < m_imagesAvail[found]->capacity()) {
           // yes, update
           found = i;
         }
@@ -283,65 +285,88 @@ std::unique_ptr<Frame::Data> SourceImpl::AllocFrame(
 
     // if nothing found, allocate a new buffer
     if (found < 0)
-      frameData.reset(new Frame::Data{size});
+      image.reset(new Image{size});
     else
-      frameData = std::move(m_framesAvail[found]);
+      image = std::move(m_imagesAvail[found]);
   }
 
-  // Initialize frame data
-  frameData->refcount = 0;
-  frameData->time = time;
-  frameData->size = size;
-  frameData->pixelFormat = pixelFormat;
-  frameData->width = width;
-  frameData->height = height;
+  // Initialize image
+  image->SetSize(size);
+  image->pixelFormat = pixelFormat;
+  image->width = width;
+  image->height = height;
 
-  return frameData;
+  return image;
 }
 
 void SourceImpl::PutFrame(VideoMode::PixelFormat pixelFormat, int width,
                           int height, llvm::StringRef data, Frame::Time time) {
-  std::unique_ptr<Frame::Data> frameData =
-      AllocFrame(pixelFormat, width, height, data.size(), time);
+  auto image = AllocImage(pixelFormat, width, height, data.size());
 
   // Copy in image data
-  SDEBUG4("Copying data to " << ((void*)frameData->data) << " from "
+  SDEBUG4("Copying data to " << ((void*)image->data()) << " from "
                              << ((void*)data.data()) << " (" << data.size()
                              << " bytes)");
-  std::memcpy(frameData->data, data.data(), data.size());
+  std::memcpy(image->data(), data.data(), data.size());
 
-  PutFrame(std::move(frameData));
+  PutFrame(std::move(image), time);
 }
 
-void SourceImpl::PutFrame(std::unique_ptr<Frame::Data> frameData) {
+void SourceImpl::PutFrame(std::unique_ptr<Image> image, Frame::Time time) {
   // Update frame
   {
     std::lock_guard<std::mutex> lock{m_frameMutex};
-    m_frame = Frame{*this, std::move(frameData)};
+    m_frame = Frame{*this, std::move(image), time};
   }
 
   // Signal listeners
   m_frameCv.notify_all();
 }
 
-void SourceImpl::ReleaseFrame(std::unique_ptr<Frame::Data> data) {
+void SourceImpl::PutError(llvm::StringRef msg, Frame::Time time) {
+  // Update frame
+  {
+    std::lock_guard<std::mutex> lock{m_frameMutex};
+    m_frame = Frame{*this, msg, time};
+  }
+
+  // Signal listeners
+  m_frameCv.notify_all();
+}
+
+void SourceImpl::ReleaseImage(std::unique_ptr<Image> image) {
   std::lock_guard<std::mutex> lock{m_poolMutex};
   if (m_destroyFrames) return;
   // Return the frame to the pool.  First try to find an empty slot, otherwise
   // add it to the end.
-  auto it = std::find(m_framesAvail.begin(), m_framesAvail.end(), nullptr);
-  if (it != m_framesAvail.end())
-    (*it) = std::move(data);
-  else if (m_framesAvail.size() > kMaxFramesAvail) {
+  auto it = std::find(m_imagesAvail.begin(), m_imagesAvail.end(), nullptr);
+  if (it != m_imagesAvail.end())
+    *it = std::move(image);
+  else if (m_imagesAvail.size() > kMaxImagesAvail) {
     // Replace smallest buffer; don't need to check for null because the above
     // find would have found it.
-    auto it2 = std::min_element(m_framesAvail.begin(), m_framesAvail.end(),
-                                [](const std::unique_ptr<Frame::Data>& a,
-                                   const std::unique_ptr<Frame::Data>& b) {
-                                  return a->capacity < b->capacity;
-                                });
-    if ((*it2)->capacity < data->capacity)
-      *it2 = std::move(data);
+    auto it2 = std::min_element(
+        m_imagesAvail.begin(), m_imagesAvail.end(),
+        [](const std::unique_ptr<Image>& a, const std::unique_ptr<Image>& b) {
+          return a->capacity() < b->capacity();
+        });
+    if ((*it2)->capacity() < image->capacity()) *it2 = std::move(image);
   } else
-    m_framesAvail.emplace_back(std::move(data));
+    m_imagesAvail.emplace_back(std::move(image));
+}
+
+std::unique_ptr<Frame::Impl> SourceImpl::AllocFrameImpl() {
+  std::lock_guard<std::mutex> lock{m_poolMutex};
+
+  if (m_framesAvail.empty()) return llvm::make_unique<Frame::Impl>(*this);
+
+  auto impl = std::move(m_framesAvail.back());
+  m_framesAvail.pop_back();
+  return impl;
+}
+
+void SourceImpl::ReleaseFrameImpl(std::unique_ptr<Frame::Impl> impl) {
+  std::lock_guard<std::mutex> lock{m_poolMutex};
+  if (m_destroyFrames) return;
+  m_framesAvail.push_back(std::move(impl));
 }

src/SourceImpl.h

@@ -21,6 +21,7 @@
 #include "llvm/StringRef.h"
 #include "cscore_cpp.h"
 #include "Frame.h"
+#include "Image.h"
 
 namespace cs {
 
@@ -117,17 +118,14 @@ class SourceImpl {
 
   std::vector<VideoMode> EnumerateVideoModes(CS_Status* status) const;
 
-  std::unique_ptr<Frame::Data> AllocFrame(VideoMode::PixelFormat pixelFormat,
-                                          int width, int height,
-                                          std::size_t size, Frame::Time time);
+  std::unique_ptr<Image> AllocImage(VideoMode::PixelFormat pixelFormat,
+                                    int width, int height, std::size_t size);
 
  protected:
   void PutFrame(VideoMode::PixelFormat pixelFormat, int width, int height,
                 llvm::StringRef data, Frame::Time time);
-  void PutFrame(std::unique_ptr<Frame::Data> frameData);
-  void PutError(llvm::StringRef msg, Frame::Time time) {
-    PutFrame(VideoMode::kUnknown, 0, 0, msg, time);
-  }
+  void PutFrame(std::unique_ptr<Image> image, Frame::Time time);
+  void PutError(llvm::StringRef msg, Frame::Time time);
 
   // Notification functions for corresponding atomics
   virtual void NumSinksChanged() = 0;
@@ -195,7 +193,9 @@ class SourceImpl {
   mutable std::mutex m_mutex;
 
  private:
-  void ReleaseFrame(std::unique_ptr<Frame::Data> data);
+  void ReleaseImage(std::unique_ptr<Image> image);
+  std::unique_ptr<Frame::Impl> AllocFrameImpl();
+  void ReleaseFrameImpl(std::unique_ptr<Frame::Impl> data);
 
   std::string m_name;
   std::string m_description;
@@ -209,9 +209,10 @@ class SourceImpl {
 
   bool m_destroyFrames{false};
 
-  // Pool of frame data to reduce malloc traffic.
+  // Pool of frames/images to reduce malloc traffic.
   std::mutex m_poolMutex;
-  std::vector<std::unique_ptr<Frame::Data>> m_framesAvail;
+  std::vector<std::unique_ptr<Frame::Impl>> m_framesAvail;
+  std::vector<std::unique_ptr<Image>> m_imagesAvail;
 
   std::atomic_bool m_connected{false};
 };

src/default_init_allocator.h

@@ -0,0 +1,38 @@
+// From: http://stackoverflow.com/questions/21028299/is-this-behavior-of-vectorresizesize-type-n-under-c11-and-boost-container
+// Credits: Casey and Howard Hinnant
+#ifndef DEFAULT_INIT_ALLOCATOR_H_
+#define DEFAULT_INIT_ALLOCATOR_H_
+
+#include <memory>
+
+namespace cs {
+
+// Allocator adaptor that interposes construct() calls to
+// convert value initialization into default initialization.
+template <typename T, typename A = std::allocator<T>>
+class default_init_allocator : public A {
+  typedef std::allocator_traits<A> a_t;
+
+ public:
+  template <typename U>
+  struct rebind {
+    using other =
+        default_init_allocator<U, typename a_t::template rebind_alloc<U>>;
+  };
+
+  using A::A;
+
+  template <typename U>
+  void construct(U* ptr) noexcept(
+      std::is_nothrow_default_constructible<U>::value) {
+    ::new (static_cast<void*>(ptr)) U;
+  }
+  template <typename U, typename... Args>
+  void construct(U* ptr, Args&&... args) {
+    a_t::construct(static_cast<A&>(*this), ptr, std::forward<Args>(args)...);
+  }
+};
+
+}  // namespace cs
+
+#endif  // DEFAULT_INIT_ALLOCATOR_H_