From de385bd1146f9ad8ff35e18808fcb38aec9e5ec0 Mon Sep 17 00:00:00 2001 From: Dale Phurrough Date: Tue, 7 Dec 2021 20:26:26 +0100 Subject: [PATCH] make RawBuffer container agnostic - easy to now change container within RawBuffer to something else like aligned vector with boost - partial fix luxonis/depthai-core#253 --- examples/ColorCamera/rgb_camera_control.cpp | 4 +- .../VideoEncoder/mjpeg_encoding_example.cpp | 2 +- examples/utility/utility.cpp | 111 +++--------------- examples/utility/utility.hpp | 68 ++++++++++- include/depthai/pipeline/datatype/Buffer.hpp | 9 +- src/pipeline/datatype/Buffer.cpp | 8 +- src/pipeline/datatype/StreamMessageParser.cpp | 6 +- tests/src/neural_network_test.cpp | 4 +- 8 files changed, 105 insertions(+), 107 deletions(-) diff --git a/examples/ColorCamera/rgb_camera_control.cpp b/examples/ColorCamera/rgb_camera_control.cpp index ac1d60a08..0c091f2bb 100644 --- a/examples/ColorCamera/rgb_camera_control.cpp +++ b/examples/ColorCamera/rgb_camera_control.cpp @@ -114,7 +114,7 @@ int main() { auto videoFrames = videoQueue->tryGetAll(); for(const auto& videoFrame : videoFrames) { // Decode JPEG - auto frame = cv::imdecode(videoFrame->getData(), cv::IMREAD_UNCHANGED); + auto frame = cv::imdecode(videoFrame->getFrame(), cv::IMREAD_UNCHANGED); // Display cv::imshow("video", frame); @@ -131,7 +131,7 @@ int main() { auto stillFrames = stillQueue->tryGetAll(); for(const auto& stillFrame : stillFrames) { // Decode JPEG - auto frame = cv::imdecode(stillFrame->getData(), cv::IMREAD_UNCHANGED); + auto frame = cv::imdecode(stillFrame->getFrame(), cv::IMREAD_UNCHANGED); // Display cv::imshow("still", frame); } diff --git a/examples/VideoEncoder/mjpeg_encoding_example.cpp b/examples/VideoEncoder/mjpeg_encoding_example.cpp index 3d4caa2a8..2c102c4de 100644 --- a/examples/VideoEncoder/mjpeg_encoding_example.cpp +++ b/examples/VideoEncoder/mjpeg_encoding_example.cpp @@ -45,7 +45,7 @@ int main() { auto t3 = steady_clock::now(); auto mjpeg = mjpegQueue->get(); auto t4 = steady_clock::now(); - cv::Mat decodedFrame = cv::imdecode(cv::Mat(mjpeg->getData()), cv::IMREAD_COLOR); + cv::Mat decodedFrame = cv::imdecode(mjpeg->getFrame(), cv::IMREAD_COLOR); auto t5 = steady_clock::now(); cv::imshow("mjpeg", decodedFrame); diff --git a/examples/utility/utility.cpp b/examples/utility/utility.cpp index 1ad8b5b6b..f594ab107 100644 --- a/examples/utility/utility.cpp +++ b/examples/utility/utility.cpp @@ -1,117 +1,42 @@ #include "utility.hpp" -// libraries -#include "fp16/fp16.h" - #include "errno.h" -#if (defined(_WIN32) || defined(_WIN64)) -#include -#include +#if(defined(_WIN32) || defined(_WIN64)) + #include + #include #else -#include + #include #endif -int createDirectory(std::string directory) -{ +int createDirectory(std::string directory) { int ret = 0; -#if (defined(_WIN32) || defined(_WIN64)) - ret = _mkdir(directory.c_str()); +#if(defined(_WIN32) || defined(_WIN64)) + ret = _mkdir(directory.c_str()); #else ret = mkdir(directory.c_str(), 0777); #endif - if (ret == EEXIST) ret = 0; + if(ret == EEXIST) ret = 0; return ret; } - -cv::Mat toMat(const std::vector& data, int w, int h , int numPlanes, int bpp){ - - cv::Mat frame; - - if(numPlanes == 3){ - frame = cv::Mat(h, w, CV_8UC3); - - // optimization (cache) - for(int i = 0; i < w*h; i++) { - uint8_t b = data.data()[i + w*h * 0]; - frame.data[i*3+0] = b; - } - for(int i = 0; i < w*h; i++) { - uint8_t g = data.data()[i + w*h * 1]; - frame.data[i*3+1] = g; - } - for(int i = 0; i < w*h; i++) { - uint8_t r = data.data()[i + w*h * 2]; - frame.data[i*3+2] = r; - } - - } else { - if(bpp == 3){ - frame = cv::Mat(h, w, CV_8UC3); - for(int i = 0; i < w*h*bpp; i+=3) { - uint8_t b,g,r; - b = data.data()[i + 2]; - g = data.data()[i + 1]; - r = data.data()[i + 0]; - frame.at( (i/bpp) / w, (i/bpp) % w) = cv::Vec3b(b,g,r); - } - - } else if(bpp == 6) { - //first denormalize - //dump - - frame = cv::Mat(h, w, CV_8UC3); - for(int y = 0; y < h; y++){ - for(int x = 0; x < w; x++){ - - const uint16_t* fp16 = (const uint16_t*) (data.data() + (y*w+x)*bpp); - uint8_t r = (uint8_t) (fp16_ieee_to_fp32_value(fp16[0]) * 255.0f); - uint8_t g = (uint8_t) (fp16_ieee_to_fp32_value(fp16[1]) * 255.0f); - uint8_t b = (uint8_t) (fp16_ieee_to_fp32_value(fp16[2]) * 255.0f); - frame.at(y, x) = cv::Vec3b(b,g,r); - } - } - - } - } - - return frame; -} - - -void toPlanar(cv::Mat& bgr, std::vector& data){ - - data.resize(bgr.cols * bgr.rows * 3); - for(int y = 0; y < bgr.rows; y++){ - for(int x = 0; x < bgr.cols; x++){ - auto p = bgr.at(y,x); - data[x + y*bgr.cols + 0 * bgr.rows*bgr.cols] = p[0]; - data[x + y*bgr.cols + 1 * bgr.rows*bgr.cols] = p[1]; - data[x + y*bgr.cols + 2 * bgr.rows*bgr.cols] = p[2]; - } - } -} - -cv::Mat resizeKeepAspectRatio(const cv::Mat &input, const cv::Size &dstSize, const cv::Scalar &bgcolor) -{ +cv::Mat resizeKeepAspectRatio(const cv::Mat& input, const cv::Size& dstSize, const cv::Scalar& bgcolor) { cv::Mat output; - double h1 = dstSize.width * (input.rows/(double)input.cols); - double w2 = dstSize.height * (input.cols/(double)input.rows); - if( h1 <= dstSize.height) { - cv::resize( input, output, cv::Size(dstSize.width, h1)); + double h1 = dstSize.width * (input.rows / (double)input.cols); + double w2 = dstSize.height * (input.cols / (double)input.rows); + if(h1 <= dstSize.height) { + cv::resize(input, output, cv::Size(dstSize.width, static_cast(h1))); } else { - cv::resize( input, output, cv::Size(w2, dstSize.height)); + cv::resize(input, output, cv::Size(static_cast(w2), dstSize.height)); } - int top = (dstSize.height-output.rows) / 2; - int down = (dstSize.height-output.rows+1) / 2; + int top = (dstSize.height - output.rows) / 2; + int down = (dstSize.height - output.rows + 1) / 2; int left = (dstSize.width - output.cols) / 2; - int right = (dstSize.width - output.cols+1) / 2; + int right = (dstSize.width - output.cols + 1) / 2; - cv::copyMakeBorder(output, output, top, down, left, right, cv::BORDER_CONSTANT, bgcolor ); + cv::copyMakeBorder(output, output, top, down, left, right, cv::BORDER_CONSTANT, bgcolor); return output; } - diff --git a/examples/utility/utility.hpp b/examples/utility/utility.hpp index 10f6facde..e517cdb37 100644 --- a/examples/utility/utility.hpp +++ b/examples/utility/utility.hpp @@ -1,8 +1,72 @@ #pragma once +#include "fp16/fp16.h" #include "opencv2/opencv.hpp" -cv::Mat toMat(const std::vector& data, int w, int h , int numPlanes, int bpp); -void toPlanar(cv::Mat& bgr, std::vector& data); +template +cv::Mat toMat(const T& data, int w, int h, int numPlanes, int bpp) { + cv::Mat frame; + + if(numPlanes == 3) { + frame = cv::Mat(h, w, CV_8UC3); + + // optimization (cache) + for(int i = 0; i < w * h; i++) { + uint8_t b = data.data()[i + w * h * 0]; + frame.data[i * 3 + 0] = b; + } + for(int i = 0; i < w * h; i++) { + uint8_t g = data.data()[i + w * h * 1]; + frame.data[i * 3 + 1] = g; + } + for(int i = 0; i < w * h; i++) { + uint8_t r = data.data()[i + w * h * 2]; + frame.data[i * 3 + 2] = r; + } + + } else { + if(bpp == 3) { + frame = cv::Mat(h, w, CV_8UC3); + for(int i = 0; i < w * h * bpp; i += 3) { + uint8_t b, g, r; + b = data.data()[i + 2]; + g = data.data()[i + 1]; + r = data.data()[i + 0]; + frame.at((i / bpp) / w, (i / bpp) % w) = cv::Vec3b(b, g, r); + } + + } else if(bpp == 6) { + // first denormalize + // dump + + frame = cv::Mat(h, w, CV_8UC3); + for(int y = 0; y < h; y++) { + for(int x = 0; x < w; x++) { + const uint16_t* fp16 = (const uint16_t*)(data.data() + (y * w + x) * bpp); + uint8_t r = (uint8_t)(fp16_ieee_to_fp32_value(fp16[0]) * 255.0f); + uint8_t g = (uint8_t)(fp16_ieee_to_fp32_value(fp16[1]) * 255.0f); + uint8_t b = (uint8_t)(fp16_ieee_to_fp32_value(fp16[2]) * 255.0f); + frame.at(y, x) = cv::Vec3b(b, g, r); + } + } + } + } + + return frame; +} + +template +void toPlanar(cv::Mat& bgr, T& data) { + data.resize(bgr.cols * bgr.rows * 3); + for(int y = 0; y < bgr.rows; y++) { + for(int x = 0; x < bgr.cols; x++) { + auto p = bgr.at(y, x); + data[x + y * bgr.cols + 0 * bgr.rows * bgr.cols] = p[0]; + data[x + y * bgr.cols + 1 * bgr.rows * bgr.cols] = p[1]; + data[x + y * bgr.cols + 2 * bgr.rows * bgr.cols] = p[2]; + } + } +} + cv::Mat resizeKeepAspectRatio(const cv::Mat &input, const cv::Size &dstSize, const cv::Scalar &bgcolor); int createDirectory(std::string directory); diff --git a/include/depthai/pipeline/datatype/Buffer.hpp b/include/depthai/pipeline/datatype/Buffer.hpp index 1efd17b5a..293880495 100644 --- a/include/depthai/pipeline/datatype/Buffer.hpp +++ b/include/depthai/pipeline/datatype/Buffer.hpp @@ -22,12 +22,17 @@ class Buffer : public ADatatype { /** * @returns Reference to internal buffer */ - std::vector& getData(); + decltype(RawBuffer::data)& getData(); /** * @param data Copies data to internal buffer */ - void setData(std::vector data); + void setData(const decltype(RawBuffer::data)& data); + + /** + * @param data Moves data to internal buffer + */ + void setData(decltype(RawBuffer::data)&& data); }; } // namespace dai diff --git a/src/pipeline/datatype/Buffer.cpp b/src/pipeline/datatype/Buffer.cpp index 38dec4430..51c95c614 100644 --- a/src/pipeline/datatype/Buffer.cpp +++ b/src/pipeline/datatype/Buffer.cpp @@ -10,11 +10,15 @@ Buffer::Buffer() : ADatatype(std::make_shared()) {} Buffer::Buffer(std::shared_ptr ptr) : ADatatype(std::move(ptr)) {} // helpers -std::vector& Buffer::getData() { +decltype(RawBuffer::data)& Buffer::getData() { return raw->data; } -void Buffer::setData(std::vector data) { +void Buffer::setData(const decltype(RawBuffer::data)& data) { + raw->data = data; +} + +void Buffer::setData(decltype(RawBuffer::data)&& data) { raw->data = std::move(data); } diff --git a/src/pipeline/datatype/StreamMessageParser.cpp b/src/pipeline/datatype/StreamMessageParser.cpp index 4932cc64c..4446d2631 100644 --- a/src/pipeline/datatype/StreamMessageParser.cpp +++ b/src/pipeline/datatype/StreamMessageParser.cpp @@ -58,7 +58,7 @@ inline int readIntLE(uint8_t* data) { } template -inline std::shared_ptr parseDatatype(nlohmann::json& ser, std::vector& data) { +inline std::shared_ptr parseDatatype(nlohmann::json& ser, decltype(RawBuffer::data)& data) { auto tmp = std::make_shared(); nlohmann::from_json(ser, *tmp); tmp->data = std::move(data); @@ -81,7 +81,7 @@ std::shared_ptr StreamMessageParser::parseMessage(streamPacketDesc_t* } // copy data part - std::vector data(packet->data, packet->data + bufferLength); + decltype(RawBuffer::data) data(packet->data, packet->data + bufferLength); // Create corresponding object switch(objectType) { @@ -173,7 +173,7 @@ std::shared_ptr StreamMessageParser::parseMessageToADatatype(streamPa } // copy data part - std::vector data(packet->data, packet->data + bufferLength); + decltype(RawBuffer::data) data(packet->data, packet->data + bufferLength); switch(objectType) { case DatatypeEnum::Buffer: { diff --git a/tests/src/neural_network_test.cpp b/tests/src/neural_network_test.cpp index 8a3879f66..a9dd4c6db 100644 --- a/tests/src/neural_network_test.cpp +++ b/tests/src/neural_network_test.cpp @@ -61,7 +61,7 @@ TEST_CASE("Neural network node data checks") { for(int i = 0; i < 10; i++) { // Setup messages dai::Buffer buffer; - buffer.setData(std::vector(MOBILENET_DATA_SIZE + i * 1024 * 10)); + buffer.setData(decltype(dai::RawBuffer::data)(MOBILENET_DATA_SIZE + i * 1024 * 10)); dai::NNData nndata1, nndata2; // Specify tensor by name @@ -73,7 +73,7 @@ TEST_CASE("Neural network node data checks") { frame.setWidth(MOBILENET_WIDTH); frame.setHeight(MOBILENET_HEIGHT); frame.setType(dai::ImgFrame::Type::BGR888p); - frame.setData(std::vector(MOBILENET_DATA_SIZE + i * 1024 * 10)); + frame.setData(decltype(dai::RawBuffer::data)(MOBILENET_DATA_SIZE + i * 1024 * 10)); int msgIndex = 0; for(const dai::ADatatype& msg : std::initializer_list>{buffer, nndata1, nndata2, frame}) {