From 80abf6bf24b82b3bba752e04211b331df6bd6be0 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Tue, 20 Dec 2016 20:48:31 -0800 Subject: [PATCH] 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 | 3 +- include/cscore_cpp.h | 3 +- java/src/edu/wpi/cscore/VideoMode.java | 2 +- src/CvSinkImpl.cpp | 29 +-- src/CvSourceImpl.cpp | 24 +- src/CvSourceImpl.h | 3 - src/Frame.cpp | 346 ++++++++++++++++++++++++- src/Frame.h | 165 +++++++----- src/Image.h | 97 +++++++ src/MjpegServerImpl.cpp | 74 ++++-- src/SinkImpl.cpp | 11 +- src/SourceImpl.cpp | 111 +++++--- src/SourceImpl.h | 21 +- src/default_init_allocator.h | 38 +++ 14 files changed, 727 insertions(+), 200 deletions(-) create mode 100644 src/Image.h create mode 100644 src/default_init_allocator.h diff --git a/include/cscore_c.h b/include/cscore_c.h index 44c09fb11d..f9ff918a6d 100644 --- a/include/cscore_c.h +++ b/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 }; // diff --git a/include/cscore_cpp.h b/include/cscore_cpp.h index b8256c166b..e6915ca620 100644 --- a/include/cscore_cpp.h +++ b/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; diff --git a/java/src/edu/wpi/cscore/VideoMode.java b/java/src/edu/wpi/cscore/VideoMode.java index 34c29176b7..9b7584f7db 100644 --- a/java/src/edu/wpi/cscore/VideoMode.java +++ b/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) { diff --git a/src/CvSinkImpl.cpp b/src/CvSinkImpl.cpp index 97d63107ba..1e962fb155 100644 --- a/src/CvSinkImpl.cpp +++ b/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(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 diff --git a/src/CvSourceImpl.cpp b/src/CvSourceImpl.cpp index 1523ae8b5d..d1911c37fc 100644 --- a/src/CvSourceImpl.cpp +++ b/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 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(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) { diff --git a/src/CvSourceImpl.h b/src/CvSourceImpl.h index b05a2fb07b..166afa566e 100644 --- a/src/CvSourceImpl.h +++ b/src/CvSourceImpl.h @@ -62,9 +62,6 @@ class CvSourceImpl : public SourceImpl { private: std::atomic_bool m_connected{true}; - std::vector m_jpegBuf; - std::vector m_compressionParams; - int m_qualityProperty; }; } // namespace cs diff --git a/src/Frame.cpp b/src/Frame.cpp index f5507480c3..be48bb3a1b 100644 --- a/src/Frame.cpp +++ b/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(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, 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 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 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 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 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 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 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 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 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)); + m_impl->images.clear(); + m_impl->source.ReleaseFrameImpl(std::unique_ptr(m_impl)); + m_impl = nullptr; } diff --git a/src/Frame.h b/src/Frame.h index 1fb1c297a3..63cf1677fc 100644 --- a/src/Frame.h +++ b/src/Frame.h @@ -10,10 +10,12 @@ #include #include +#include -#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 images; + std::vector compressionParams; }; public: - Frame() noexcept : m_source{nullptr}, m_data{nullptr} {} + Frame() noexcept : m_impl{nullptr} {} - Frame(SourceImpl& source, std::unique_ptr 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, 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 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 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 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 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 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 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 diff --git a/src/Image.h b/src/Image.h new file mode 100644 index 0000000000..ea17eaf1ce --- /dev/null +++ b/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 + +#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{}} { + 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(m_data.data()); + } + char* data() { return reinterpret_cast(m_data.data()); } + std::size_t size() const { return m_data.size(); } + + const std::vector& vec() const { return m_data; } + std::vector& 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 m_data; + + public: + VideoMode::PixelFormat pixelFormat{VideoMode::kUnknown}; + int width{0}; + int height{0}; +}; + +} // namespace cs + +#endif // CS_IMAGE_H_ diff --git a/src/MjpegServerImpl.cpp b/src/MjpegServerImpl.cpp index 2148fc1221..b6b989a33a 100644 --- a/src/MjpegServerImpl.cpp +++ b/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; diff --git a/src/SinkImpl.cpp b/src/SinkImpl.cpp index 977d4988cb..f843c6fe1c 100644 --- a/src/SinkImpl.cpp +++ b/src/SinkImpl.cpp @@ -81,17 +81,16 @@ void SinkImpl::SetSource(std::shared_ptr source) { std::string SinkImpl::GetError() const { std::lock_guard 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& buf) const { std::lock_guard 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()}; } diff --git a/src/SourceImpl.cpp b/src/SourceImpl.cpp index 8bcac77235..9e1ada29d4 100644 --- a/src/SourceImpl.cpp +++ b/src/SourceImpl.cpp @@ -10,15 +10,17 @@ #include #include +#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 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 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 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 SourceImpl::EnumerateVideoModes( return m_videoModes; } -std::unique_ptr SourceImpl::AllocFrame( - VideoMode::PixelFormat pixelFormat, int width, int height, std::size_t size, - Frame::Time time) { - std::unique_ptr frameData; +std::unique_ptr SourceImpl::AllocImage( + VideoMode::PixelFormat pixelFormat, int width, int height, + std::size_t size) { + std::unique_ptr image; { std::lock_guard 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 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 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 frameData) { +void SourceImpl::PutFrame(std::unique_ptr image, Frame::Time time) { // Update frame { std::lock_guard 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 data) { +void SourceImpl::PutError(llvm::StringRef msg, Frame::Time time) { + // Update frame + { + std::lock_guard lock{m_frameMutex}; + m_frame = Frame{*this, msg, time}; + } + + // Signal listeners + m_frameCv.notify_all(); +} + +void SourceImpl::ReleaseImage(std::unique_ptr image) { std::lock_guard 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& a, - const std::unique_ptr& 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& a, const std::unique_ptr& 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 SourceImpl::AllocFrameImpl() { + std::lock_guard lock{m_poolMutex}; + + if (m_framesAvail.empty()) return llvm::make_unique(*this); + + auto impl = std::move(m_framesAvail.back()); + m_framesAvail.pop_back(); + return impl; +} + +void SourceImpl::ReleaseFrameImpl(std::unique_ptr impl) { + std::lock_guard lock{m_poolMutex}; + if (m_destroyFrames) return; + m_framesAvail.push_back(std::move(impl)); } diff --git a/src/SourceImpl.h b/src/SourceImpl.h index 9a769d15fb..0d0e87d2a5 100644 --- a/src/SourceImpl.h +++ b/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 EnumerateVideoModes(CS_Status* status) const; - std::unique_ptr AllocFrame(VideoMode::PixelFormat pixelFormat, - int width, int height, - std::size_t size, Frame::Time time); + std::unique_ptr 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 frameData); - void PutError(llvm::StringRef msg, Frame::Time time) { - PutFrame(VideoMode::kUnknown, 0, 0, msg, time); - } + void PutFrame(std::unique_ptr 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 data); + void ReleaseImage(std::unique_ptr image); + std::unique_ptr AllocFrameImpl(); + void ReleaseFrameImpl(std::unique_ptr 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> m_framesAvail; + std::vector> m_framesAvail; + std::vector> m_imagesAvail; std::atomic_bool m_connected{false}; }; diff --git a/src/default_init_allocator.h b/src/default_init_allocator.h new file mode 100644 index 0000000000..8c6e7c5259 --- /dev/null +++ b/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 + +namespace cs { + +// Allocator adaptor that interposes construct() calls to +// convert value initialization into default initialization. +template > +class default_init_allocator : public A { + typedef std::allocator_traits a_t; + + public: + template + struct rebind { + using other = + default_init_allocator>; + }; + + using A::A; + + template + void construct(U* ptr) noexcept( + std::is_nothrow_default_constructible::value) { + ::new (static_cast(ptr)) U; + } + template + void construct(U* ptr, Args&&... args) { + a_t::construct(static_cast(*this), ptr, std::forward(args)...); + } +}; + +} // namespace cs + +#endif // DEFAULT_INIT_ALLOCATOR_H_