obsdk demo：stream_color
This commit is contained in:
tangchao0503
653
ConsoleApplication1/utils_opencv.cpp
Normal file
653
ConsoleApplication1/utils_opencv.cpp
Normal file
@ -0,0 +1,653 @@
|
||||
// Copyright (c) Orbbec Inc. All Rights Reserved.
|
||||
// Licensed under the MIT License.
|
||||
|
||||
#include "utils_opencv.hpp"
|
||||
#include "utils.hpp"
|
||||
#include "utils_types.h"
|
||||
|
||||
#if defined(__has_include)
|
||||
#if __has_include(<opencv2/core/utils/logger.hpp>)
|
||||
#include <opencv2/core/utils/logger.hpp>
|
||||
#define TO_DISABLE_OPENCV_LOG
|
||||
#endif
|
||||
#endif
|
||||
|
||||
namespace ob_smpl {
|
||||
|
||||
const std::string defaultKeyMapPrompt = "'Esc': Exit Window, '?': Show Key Map";
|
||||
CVWindow::CVWindow(std::string name, uint32_t width, uint32_t height, ArrangeMode arrangeMode)
|
||||
: name_(std::move(name)),
|
||||
arrangeMode_(arrangeMode),
|
||||
width_(width),
|
||||
height_(height),
|
||||
closed_(false),
|
||||
showInfo_(true),
|
||||
showSyncTimeInfo_(false),
|
||||
isWindowDestroyed_(false),
|
||||
alpha_(0.6f),
|
||||
showPrompt_(false) {
|
||||
|
||||
#if defined(TO_DISABLE_OPENCV_LOG)
|
||||
cv::utils::logging::setLogLevel(cv::utils::logging::LogLevel::LOG_LEVEL_SILENT);
|
||||
#endif
|
||||
|
||||
prompt_ = defaultKeyMapPrompt;
|
||||
|
||||
cv::namedWindow(name_, cv::WINDOW_NORMAL);
|
||||
cv::resizeWindow(name_, width_, height_);
|
||||
|
||||
renderMat_ = cv::Mat::zeros(height_, width_, CV_8UC3);
|
||||
cv::putText(renderMat_, "Waiting for streams...", cv::Point(8, 16), cv::FONT_HERSHEY_DUPLEX, 0.5, cv::Scalar(255, 255, 255), 1, cv::LINE_AA);
|
||||
cv::imshow(name_, renderMat_);
|
||||
|
||||
// start processing thread
|
||||
processThread_ = std::thread(&CVWindow::processFrames, this);
|
||||
|
||||
winCreatedTime_ = getNowTimesMs();
|
||||
}
|
||||
|
||||
CVWindow::~CVWindow() noexcept {
|
||||
close();
|
||||
destroyWindow();
|
||||
}
|
||||
|
||||
void CVWindow::setKeyPressedCallback(std::function<void(int)> callback) {
|
||||
keyPressedCallback_ = callback;
|
||||
}
|
||||
|
||||
// if window is closed
|
||||
bool CVWindow::run() {
|
||||
|
||||
{
|
||||
// show render mat
|
||||
std::lock_guard<std::mutex> lock(renderMatsMtx_);
|
||||
cv::imshow(name_, renderMat_);
|
||||
}
|
||||
|
||||
int key = cv::waitKey(1);
|
||||
if(key != -1) {
|
||||
if(key == ESC_KEY) {
|
||||
closed_ = true;
|
||||
srcFrameGroupsCv_.notify_all();
|
||||
}
|
||||
else if(key == '1') {
|
||||
arrangeMode_ = ARRANGE_SINGLE;
|
||||
addLog("Switch to SINGLE arrange mode");
|
||||
}
|
||||
else if(key == '2') {
|
||||
arrangeMode_ = ARRANGE_ONE_ROW;
|
||||
addLog("Switch to ONE_ROW arrange mode");
|
||||
}
|
||||
else if(key == '3') {
|
||||
arrangeMode_ = ARRANGE_ONE_COLUMN;
|
||||
addLog("Switch to ONE_COLUMN arrange mode");
|
||||
}
|
||||
else if(key == '4') {
|
||||
arrangeMode_ = ARRANGE_GRID;
|
||||
addLog("Switch to GRID arrange mode");
|
||||
}
|
||||
else if(key == '5') {
|
||||
arrangeMode_ = ARRANGE_OVERLAY;
|
||||
addLog("Switch to OVERLAY arrange mode");
|
||||
}
|
||||
else if(key == '?' || key == '/') {
|
||||
showPrompt_ = !showPrompt_;
|
||||
}
|
||||
else if(key == '+' || key == '=') {
|
||||
alpha_ += 0.1f;
|
||||
if(alpha_ > 1) {
|
||||
alpha_ = 1;
|
||||
}
|
||||
addLog("Adjust alpha to " + ob_smpl::toString(alpha_, 1) + " (Only valid in OVERLAY arrange mode)");
|
||||
}
|
||||
else if(key == '-' || key == '_') {
|
||||
alpha_ -= 0.1f;
|
||||
if(alpha_ < 0) {
|
||||
alpha_ = 0;
|
||||
}
|
||||
addLog("Adjust alpha to " + ob_smpl::toString(alpha_, 1) + " (Only valid in OVERLAY arrange mode)");
|
||||
}
|
||||
if(keyPressedCallback_) {
|
||||
keyPressedCallback_(key);
|
||||
}
|
||||
}
|
||||
return !closed_;
|
||||
}
|
||||
|
||||
// close window
|
||||
void CVWindow::close() {
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(renderMatsMtx_);
|
||||
closed_ = true;
|
||||
srcFrameGroupsCv_.notify_all();
|
||||
}
|
||||
|
||||
if(processThread_.joinable()) {
|
||||
processThread_.join();
|
||||
}
|
||||
|
||||
matGroups_.clear();
|
||||
srcFrameGroups_.clear();
|
||||
}
|
||||
|
||||
void CVWindow::destroyWindow() {
|
||||
if(!isWindowDestroyed_) {
|
||||
cv::destroyWindow(name_);
|
||||
cv::waitKey(1);
|
||||
isWindowDestroyed_ = true;
|
||||
}
|
||||
else {
|
||||
std::cout << "CVWindows has been destroyed!" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void CVWindow::reset() {
|
||||
// close thread and clear cache
|
||||
close();
|
||||
|
||||
// restart thread
|
||||
closed_ = false;
|
||||
processThread_ = std::thread(&CVWindow::processFrames, this);
|
||||
}
|
||||
|
||||
// set the window size
|
||||
void CVWindow::resize(int width, int height) {
|
||||
width_ = width;
|
||||
height_ = height;
|
||||
cv::resizeWindow(name_, width_, height_);
|
||||
}
|
||||
|
||||
void CVWindow::setKeyPrompt(const std::string &prompt) {
|
||||
prompt_ = defaultKeyMapPrompt + ", " + prompt;
|
||||
}
|
||||
|
||||
void CVWindow::addLog(const std::string &log) {
|
||||
log_ = log;
|
||||
logCreatedTime_ = getNowTimesMs();
|
||||
}
|
||||
|
||||
// add frames to the show
|
||||
void CVWindow::pushFramesToView(std::vector<std::shared_ptr<const ob::Frame>> frames, int groupId) {
|
||||
if(frames.empty()) {
|
||||
return;
|
||||
}
|
||||
|
||||
std::vector<std::shared_ptr<const ob::Frame>> singleFrames;
|
||||
for(auto &frame: frames) {
|
||||
if(frame == nullptr) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if(!frame->is<ob::FrameSet>()) {
|
||||
// single frame, add to the list
|
||||
singleFrames.push_back(frame);
|
||||
continue;
|
||||
}
|
||||
|
||||
// FrameSet contains multiple frames
|
||||
auto frameSet = frame->as<ob::FrameSet>();
|
||||
for(uint32_t index = 0; index < frameSet->getCount(); index++) {
|
||||
auto subFrame = frameSet->getFrameByIndex(index);
|
||||
singleFrames.push_back(subFrame);
|
||||
}
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lk(srcFrameGroupsMtx_);
|
||||
srcFrameGroups_[groupId] = singleFrames;
|
||||
srcFrameGroupsCv_.notify_one();
|
||||
}
|
||||
|
||||
void CVWindow::pushFramesToView(std::shared_ptr<const ob::Frame> currentFrame, int groupId) {
|
||||
pushFramesToView(std::vector<std::shared_ptr<const ob::Frame>>{ currentFrame }, groupId);
|
||||
}
|
||||
|
||||
// set show frame info
|
||||
void CVWindow::setShowInfo(bool show) {
|
||||
showInfo_ = show;
|
||||
}
|
||||
|
||||
// set show frame synctime info
|
||||
void CVWindow::setShowSyncTimeInfo(bool show) {
|
||||
showSyncTimeInfo_ = show;
|
||||
}
|
||||
// set alpha for OVERLAY render mode
|
||||
void CVWindow::setAlpha(float alpha) {
|
||||
alpha_ = alpha;
|
||||
if(alpha_ < 0) {
|
||||
alpha_ = 0;
|
||||
}
|
||||
else if(alpha_ > 1) {
|
||||
alpha_ = 1;
|
||||
}
|
||||
}
|
||||
|
||||
// frames processing thread
|
||||
void CVWindow::processFrames() {
|
||||
std::map<int, std::vector<std::shared_ptr<const ob::Frame>>> frameGroups;
|
||||
while(!closed_) {
|
||||
if(closed_) {
|
||||
break;
|
||||
}
|
||||
{
|
||||
std::unique_lock<std::mutex> lk(srcFrameGroupsMtx_);
|
||||
srcFrameGroupsCv_.wait(lk);
|
||||
frameGroups = srcFrameGroups_;
|
||||
}
|
||||
|
||||
if(frameGroups.empty()) {
|
||||
continue;
|
||||
}
|
||||
|
||||
for(const auto &framesItem: frameGroups) {
|
||||
int groupId = framesItem.first;
|
||||
const auto &frames = framesItem.second;
|
||||
for(const auto &frame: frames) {
|
||||
auto rstMat = visualize(frame);
|
||||
if(!rstMat.empty()) {
|
||||
int uid = groupId * OB_FRAME_TYPE_COUNT + static_cast<int>(frame->getType());
|
||||
matGroups_[uid] = { frame, rstMat };
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(matGroups_.empty()) {
|
||||
continue;
|
||||
}
|
||||
|
||||
arrangeFrames();
|
||||
}
|
||||
}
|
||||
|
||||
void CVWindow::arrangeFrames() {
|
||||
cv::Mat renderMat;
|
||||
try {
|
||||
if(arrangeMode_ == ARRANGE_SINGLE || matGroups_.size() == 1) {
|
||||
auto &mat = matGroups_.begin()->second.second;
|
||||
renderMat = resizeMatKeepAspectRatio(mat, width_, height_);
|
||||
}
|
||||
else if(arrangeMode_ == ARRANGE_ONE_ROW) {
|
||||
for(auto &item: matGroups_) {
|
||||
auto &mat = item.second.second;
|
||||
cv::Mat resizeMat = resizeMatKeepAspectRatio(mat, static_cast<int>(width_ / matGroups_.size()), height_);
|
||||
if(renderMat.dims > 0 && renderMat.cols > 0 && renderMat.rows > 0) {
|
||||
cv::hconcat(renderMat, resizeMat, renderMat);
|
||||
}
|
||||
else {
|
||||
renderMat = resizeMat;
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(arrangeMode_ == ARRANGE_ONE_COLUMN) {
|
||||
for(auto &item: matGroups_) {
|
||||
auto &mat = item.second.second;
|
||||
cv::Mat resizeMat = resizeMatKeepAspectRatio(mat, width_, static_cast<int>(height_ / matGroups_.size()));
|
||||
if(renderMat.dims > 0 && renderMat.cols > 0 && renderMat.rows > 0) {
|
||||
cv::vconcat(renderMat, resizeMat, renderMat);
|
||||
}
|
||||
else {
|
||||
renderMat = resizeMat;
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(arrangeMode_ == ARRANGE_GRID) {
|
||||
int count = static_cast<int>(matGroups_.size());
|
||||
int idealSide = static_cast<int>(std::sqrt(count));
|
||||
int rows = idealSide;
|
||||
int cols = idealSide;
|
||||
while(rows * cols < count) { // find the best row and column count
|
||||
cols++;
|
||||
if(rows * cols < count) {
|
||||
rows++;
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<cv::Mat> gridImages; // store all images in grid
|
||||
auto it = matGroups_.begin();
|
||||
for(int i = 0; i < rows; i++) {
|
||||
std::vector<cv::Mat> rowImages; // store images in the same row
|
||||
for(int j = 0; j < cols; j++) {
|
||||
int index = i * cols + j;
|
||||
cv::Mat resizeMat;
|
||||
if(index < count) {
|
||||
auto mat = it->second.second;
|
||||
resizeMat = resizeMatKeepAspectRatio(mat, width_ / cols, height_ / rows);
|
||||
it++;
|
||||
}
|
||||
else {
|
||||
resizeMat = cv::Mat::zeros(height_ / rows, width_ / cols, CV_8UC3); // fill with black
|
||||
}
|
||||
rowImages.push_back(resizeMat);
|
||||
}
|
||||
cv::Mat lineMat;
|
||||
cv::hconcat(rowImages, lineMat); // horizontal concat all images in the same row
|
||||
gridImages.push_back(lineMat);
|
||||
}
|
||||
|
||||
cv::vconcat(gridImages, renderMat); // vertical concat all images in the grid
|
||||
}
|
||||
else if(arrangeMode_ == ARRANGE_OVERLAY && matGroups_.size() >= 2) {
|
||||
cv::Mat overlayMat;
|
||||
const auto &mat1 = matGroups_.begin()->second.second;
|
||||
const auto &mat2 = matGroups_.rbegin()->second.second;
|
||||
renderMat = resizeMatKeepAspectRatio(mat1, width_, height_);
|
||||
overlayMat = resizeMatKeepAspectRatio(mat2, width_, height_);
|
||||
|
||||
float alpha = alpha_;
|
||||
for(int i = 0; i < renderMat.rows; i++) {
|
||||
for(int j = 0; j < renderMat.cols; j++) {
|
||||
cv::Vec3b &outRgb = renderMat.at<cv::Vec3b>(i, j);
|
||||
cv::Vec3b &overlayRgb = overlayMat.at<cv::Vec3b>(i, j);
|
||||
|
||||
outRgb[0] = (uint8_t)(outRgb[0] * (1.0f - alpha) + overlayRgb[0] * alpha);
|
||||
outRgb[1] = (uint8_t)(outRgb[1] * (1.0f - alpha) + overlayRgb[1] * alpha);
|
||||
outRgb[2] = (uint8_t)(outRgb[2] * (1.0f - alpha) + overlayRgb[2] * alpha);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
catch(std::exception &e) {
|
||||
std::cerr << e.what() << std::endl;
|
||||
}
|
||||
|
||||
if(renderMat.empty()) {
|
||||
return;
|
||||
}
|
||||
|
||||
if(showPrompt_ || getNowTimesMs() - winCreatedTime_ < 5000) {
|
||||
cv::putText(renderMat, prompt_, cv::Point(8, 16), cv::FONT_HERSHEY_DUPLEX, 0.5, cv::Scalar(255, 255, 255), 1, cv::LINE_AA);
|
||||
}
|
||||
|
||||
if(!log_.empty() && getNowTimesMs() - logCreatedTime_ < 3000) {
|
||||
cv::putText(renderMat, log_, cv::Point(8, height_ - 16), cv::FONT_HERSHEY_DUPLEX, 0.5, cv::Scalar(255, 255, 255), 1, cv::LINE_AA);
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(renderMatsMtx_);
|
||||
renderMat_ = renderMat;
|
||||
}
|
||||
|
||||
cv::Mat CVWindow::visualize(std::shared_ptr<const ob::Frame> frame) {
|
||||
if(frame == nullptr) {
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
cv::Mat rstMat;
|
||||
switch(frame->getType()) {
|
||||
case OB_FRAME_COLOR:
|
||||
case OB_FRAME_COLOR_LEFT:
|
||||
case OB_FRAME_COLOR_RIGHT: {
|
||||
auto videoFrame = frame->as<const ob::VideoFrame>();
|
||||
switch(videoFrame->getFormat()) {
|
||||
case OB_FORMAT_MJPG: {
|
||||
cv::Mat rawMat(1, videoFrame->getDataSize(), CV_8UC1, videoFrame->getData());
|
||||
rstMat = cv::imdecode(rawMat, 1);
|
||||
} break;
|
||||
case OB_FORMAT_NV21: {
|
||||
cv::Mat rawMat(videoFrame->getHeight() * 3 / 2, videoFrame->getWidth(), CV_8UC1, videoFrame->getData());
|
||||
cv::cvtColor(rawMat, rstMat, cv::COLOR_YUV2BGR_NV21);
|
||||
} break;
|
||||
case OB_FORMAT_YUYV:
|
||||
case OB_FORMAT_YUY2: {
|
||||
cv::Mat rawMat(videoFrame->getHeight(), videoFrame->getWidth(), CV_8UC2, videoFrame->getData());
|
||||
cv::cvtColor(rawMat, rstMat, cv::COLOR_YUV2BGR_YUY2);
|
||||
} break;
|
||||
case OB_FORMAT_BGR: {
|
||||
cv::Mat rawMat(videoFrame->getHeight(), videoFrame->getWidth(), CV_8UC3, videoFrame->getData());
|
||||
cv::cvtColor(rawMat, rstMat, cv::COLOR_BGR2RGB);
|
||||
} break;
|
||||
case OB_FORMAT_RGB: {
|
||||
cv::Mat rawMat(videoFrame->getHeight(), videoFrame->getWidth(), CV_8UC3, videoFrame->getData());
|
||||
cv::cvtColor(rawMat, rstMat, cv::COLOR_RGB2BGR);
|
||||
} break;
|
||||
case OB_FORMAT_RGBA: {
|
||||
cv::Mat rawMat(videoFrame->getHeight(), videoFrame->getWidth(), CV_8UC4, videoFrame->getData());
|
||||
cv::cvtColor(rawMat, rstMat, cv::COLOR_RGBA2BGR);
|
||||
} break;
|
||||
case OB_FORMAT_BGRA: {
|
||||
cv::Mat rawMat(videoFrame->getHeight(), videoFrame->getWidth(), CV_8UC4, videoFrame->getData());
|
||||
cv::cvtColor(rawMat, rstMat, cv::COLOR_BGRA2RGB);
|
||||
} break;
|
||||
case OB_FORMAT_UYVY: {
|
||||
cv::Mat rawMat(videoFrame->getHeight(), videoFrame->getWidth(), CV_8UC2, videoFrame->getData());
|
||||
cv::cvtColor(rawMat, rstMat, cv::COLOR_YUV2BGR_UYVY);
|
||||
} break;
|
||||
case OB_FORMAT_I420: {
|
||||
cv::Mat rawMat(videoFrame->getHeight() * 3 / 2, videoFrame->getWidth(), CV_8UC1, videoFrame->getData());
|
||||
cv::cvtColor(rawMat, rstMat, cv::COLOR_YUV2BGR_I420);
|
||||
} break;
|
||||
case OB_FORMAT_Y8: {
|
||||
cv::Mat rawMat(videoFrame->getHeight(), videoFrame->getWidth(), CV_8UC1, videoFrame->getData());
|
||||
cv::cvtColor(rawMat, rstMat, cv::COLOR_GRAY2BGR);
|
||||
} break;
|
||||
case OB_FORMAT_Y16: {
|
||||
cv::Mat rawMat(videoFrame->getHeight(), videoFrame->getWidth(), CV_16UC1, videoFrame->getData());
|
||||
cv::Mat gray8;
|
||||
rawMat.convertTo(gray8, CV_8UC1, 255.0 / 65535.0);
|
||||
cv::cvtColor(gray8, rstMat, cv::COLOR_GRAY2BGR);
|
||||
} break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
if(showSyncTimeInfo_ && !rstMat.empty()) {
|
||||
drawInfo(rstMat, videoFrame);
|
||||
}
|
||||
} break;
|
||||
case OB_FRAME_DEPTH: {
|
||||
auto videoFrame = frame->as<const ob::VideoFrame>();
|
||||
if(videoFrame->getFormat() == OB_FORMAT_Y16 || videoFrame->getFormat() == OB_FORMAT_Z16 || videoFrame->getFormat() == OB_FORMAT_Y12C4) {
|
||||
cv::Mat rawMat = cv::Mat(videoFrame->getHeight(), videoFrame->getWidth(), CV_16UC1, videoFrame->getData());
|
||||
// depth frame pixel value multiply scale to get distance in millimeter
|
||||
float scale = videoFrame->as<ob::DepthFrame>()->getValueScale();
|
||||
|
||||
cv::Mat cvtMat;
|
||||
// normalization to 0-255. 0.032f is 256/8000, to limit the range of depth to 8000mm
|
||||
rawMat.convertTo(cvtMat, CV_32F, scale * 0.032f);
|
||||
|
||||
// apply gamma correction to enhance the contrast for near objects
|
||||
cv::pow(cvtMat, 0.6f, cvtMat);
|
||||
|
||||
// convert to 8-bit
|
||||
cvtMat.convertTo(cvtMat, CV_8UC1, 10); // multiplier 10 is to normalize to 0-255 (nearly) after applying gamma correction
|
||||
|
||||
// apply colormap
|
||||
cv::applyColorMap(cvtMat, rstMat, cv::COLORMAP_JET);
|
||||
}
|
||||
if(showSyncTimeInfo_ && !rstMat.empty()) {
|
||||
drawInfo(rstMat, videoFrame);
|
||||
}
|
||||
} break;
|
||||
case OB_FRAME_IR:
|
||||
case OB_FRAME_IR_LEFT:
|
||||
case OB_FRAME_IR_RIGHT: {
|
||||
auto videoFrame = frame->as<const ob::VideoFrame>();
|
||||
if(videoFrame->getFormat() == OB_FORMAT_Y16) {
|
||||
cv::Mat cvtMat;
|
||||
cv::Mat rawMat = cv::Mat(videoFrame->getHeight(), videoFrame->getWidth(), CV_16UC1, videoFrame->getData());
|
||||
rawMat.convertTo(cvtMat, CV_8UC1, 1.0 / 16.0f);
|
||||
cv::cvtColor(cvtMat, rstMat, cv::COLOR_GRAY2RGB);
|
||||
}
|
||||
else if(videoFrame->getFormat() == OB_FORMAT_Y8) {
|
||||
cv::Mat rawMat = cv::Mat(videoFrame->getHeight(), videoFrame->getWidth(), CV_8UC1, videoFrame->getData());
|
||||
cv::cvtColor(rawMat, rstMat, cv::COLOR_GRAY2RGB);
|
||||
}
|
||||
else if(videoFrame->getFormat() == OB_FORMAT_MJPG) {
|
||||
cv::Mat rawMat(1, videoFrame->getDataSize(), CV_8UC1, videoFrame->getData());
|
||||
rstMat = cv::imdecode(rawMat, 1);
|
||||
cv::cvtColor(rstMat, rstMat, cv::COLOR_GRAY2RGB);
|
||||
}
|
||||
if(showSyncTimeInfo_ && !rstMat.empty()) {
|
||||
drawInfo(rstMat, videoFrame);
|
||||
}
|
||||
} break;
|
||||
case OB_FRAME_CONFIDENCE: {
|
||||
auto videoFrame = frame->as<const ob::VideoFrame>();
|
||||
if(videoFrame->getFormat() == OB_FORMAT_Y16) {
|
||||
cv::Mat cvtMat;
|
||||
cv::Mat rawMat = cv::Mat(videoFrame->getHeight(), videoFrame->getWidth(), CV_16UC1, videoFrame->getData());
|
||||
rawMat.convertTo(cvtMat, CV_8UC1, 1.0 / 16.0f);
|
||||
cv::cvtColor(cvtMat, rstMat, cv::COLOR_GRAY2RGB);
|
||||
}
|
||||
else if(videoFrame->getFormat() == OB_FORMAT_Y8) {
|
||||
cv::Mat rawMat = cv::Mat(videoFrame->getHeight(), videoFrame->getWidth(), CV_8UC1, videoFrame->getData());
|
||||
cv::cvtColor(rawMat, rstMat, cv::COLOR_GRAY2RGB);
|
||||
}
|
||||
} break;
|
||||
case OB_FRAME_ACCEL: {
|
||||
rstMat = cv::Mat::zeros(320, 300, CV_8UC3);
|
||||
auto accelFrame = frame->as<ob::AccelFrame>();
|
||||
auto value = accelFrame->getValue();
|
||||
std::string str = "Accel:";
|
||||
cv::putText(rstMat, str.c_str(), cv::Point(8, 60), cv::FONT_HERSHEY_DUPLEX, 0.5, cv::Scalar(255, 255, 255), 1, cv::LINE_AA);
|
||||
str = std::string(" timestamp=") + std::to_string(accelFrame->getTimeStampUs()) + "us";
|
||||
cv::putText(rstMat, str.c_str(), cv::Point(8, 100), cv::FONT_HERSHEY_DUPLEX, 0.5, cv::Scalar(255, 255, 255), 1, cv::LINE_AA);
|
||||
str = std::string(" x=") + std::to_string(value.x) + "m/s^2";
|
||||
cv::putText(rstMat, str.c_str(), cv::Point(8, 140), cv::FONT_HERSHEY_DUPLEX, 0.5, cv::Scalar(255, 255, 255), 1, cv::LINE_AA);
|
||||
str = std::string(" y=") + std::to_string(value.y) + "m/s^2";
|
||||
cv::putText(rstMat, str.c_str(), cv::Point(8, 180), cv::FONT_HERSHEY_DUPLEX, 0.5, cv::Scalar(255, 255, 255), 1, cv::LINE_AA);
|
||||
str = std::string(" z=") + std::to_string(value.z) + "m/s^2";
|
||||
cv::putText(rstMat, str.c_str(), cv::Point(8, 220), cv::FONT_HERSHEY_DUPLEX, 0.5, cv::Scalar(255, 255, 255), 1, cv::LINE_AA);
|
||||
} break;
|
||||
case OB_FRAME_GYRO: {
|
||||
rstMat = cv::Mat::zeros(320, 300, CV_8UC3);
|
||||
auto gyroFrame = frame->as<ob::GyroFrame>();
|
||||
auto value = gyroFrame->getValue();
|
||||
std::string str = "Gyro:";
|
||||
cv::putText(rstMat, str.c_str(), cv::Point(8, 60), cv::FONT_HERSHEY_DUPLEX, 0.5, cv::Scalar(255, 255, 255), 1, cv::LINE_AA);
|
||||
str = std::string(" timestamp=") + std::to_string(gyroFrame->getTimeStampUs()) + "us";
|
||||
cv::putText(rstMat, str.c_str(), cv::Point(8, 100), cv::FONT_HERSHEY_DUPLEX, 0.5, cv::Scalar(255, 255, 255), 1, cv::LINE_AA);
|
||||
str = std::string(" x=") + std::to_string(value.x) + "rad/s";
|
||||
cv::putText(rstMat, str.c_str(), cv::Point(8, 140), cv::FONT_HERSHEY_DUPLEX, 0.5, cv::Scalar(255, 255, 255), 1, cv::LINE_AA);
|
||||
str = std::string(" y=") + std::to_string(value.y) + "rad/s";
|
||||
cv::putText(rstMat, str.c_str(), cv::Point(8, 180), cv::FONT_HERSHEY_DUPLEX, 0.5, cv::Scalar(255, 255, 255), 1, cv::LINE_AA);
|
||||
str = std::string(" z=") + std::to_string(value.z) + "rad/s";
|
||||
cv::putText(rstMat, str.c_str(), cv::Point(8, 220), cv::FONT_HERSHEY_DUPLEX, 0.5, cv::Scalar(255, 255, 255), 1, cv::LINE_AA);
|
||||
} break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
return rstMat;
|
||||
}
|
||||
|
||||
// add frame info to mat
|
||||
void CVWindow::drawInfo(cv::Mat &imageMat, std::shared_ptr<const ob::VideoFrame> &frame) {
|
||||
int baseline = 0; // Used to calculate text size and baseline
|
||||
cv::Size textSize; // Size of the text to be drawn
|
||||
int padding = 5; // Padding around the text for the background
|
||||
|
||||
// Helper lambda function to draw text with background
|
||||
auto putTextWithBackground = [&](const std::string &text, cv::Point origin) {
|
||||
// Getting text size for background
|
||||
textSize = cv::getTextSize(text, cv::FONT_HERSHEY_SIMPLEX, 0.6, 1, &baseline);
|
||||
|
||||
// Drawing the white background
|
||||
cv::rectangle(imageMat, origin + cv::Point(0, baseline), origin + cv::Point(textSize.width, -textSize.height) - cv::Point(0, padding),
|
||||
cv::Scalar(255, 255, 255), cv::FILLED);
|
||||
|
||||
// Putting black text on the white background
|
||||
cv::putText(imageMat, text, origin, cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 0, 0), 1);
|
||||
};
|
||||
|
||||
// Drawing text with background based on frame type
|
||||
auto frameType = frame->getType();
|
||||
auto frameFormat = frame->getFormat();
|
||||
switch(frameFormat) {
|
||||
case OB_FORMAT_NV21: {
|
||||
switch(frameType) {
|
||||
case OB_FRAME_COLOR:
|
||||
putTextWithBackground("Color-NV21", cv::Point(8, 16));
|
||||
break;
|
||||
case OB_FRAME_COLOR_LEFT:
|
||||
putTextWithBackground("LeftColor-NV21", cv::Point(8, 16));
|
||||
break;
|
||||
case OB_FRAME_COLOR_RIGHT:
|
||||
putTextWithBackground("RightColor-NV21", cv::Point(8, 16));
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
} break;
|
||||
case OB_FORMAT_MJPG: {
|
||||
switch(frameType) {
|
||||
case OB_FRAME_COLOR:
|
||||
putTextWithBackground("Color-MJPG", cv::Point(8, 16));
|
||||
break;
|
||||
case OB_FRAME_COLOR_LEFT:
|
||||
putTextWithBackground("LeftColor-MJPG", cv::Point(8, 16));
|
||||
break;
|
||||
case OB_FRAME_COLOR_RIGHT:
|
||||
putTextWithBackground("RightColor-MJPG", cv::Point(8, 16));
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
} break;
|
||||
case OB_FORMAT_YUYV:
|
||||
case OB_FORMAT_YUY2: {
|
||||
switch(frameType) {
|
||||
case OB_FRAME_COLOR:
|
||||
putTextWithBackground("Color-YUYV", cv::Point(8, 16));
|
||||
break;
|
||||
case OB_FRAME_COLOR_LEFT:
|
||||
putTextWithBackground("LeftColor-YUYV", cv::Point(8, 16));
|
||||
break;
|
||||
case OB_FRAME_COLOR_RIGHT:
|
||||
putTextWithBackground("RightColor-YUYV", cv::Point(8, 16));
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
} break;
|
||||
default: {
|
||||
switch(frameType) {
|
||||
case OB_FRAME_DEPTH:
|
||||
putTextWithBackground("Depth", cv::Point(8, 16));
|
||||
break;
|
||||
case OB_FRAME_IR:
|
||||
putTextWithBackground("IR", cv::Point(8, 16));
|
||||
break;
|
||||
case OB_FRAME_IR_LEFT:
|
||||
putTextWithBackground("LeftIR", cv::Point(8, 16));
|
||||
break;
|
||||
case OB_FRAME_IR_RIGHT:
|
||||
putTextWithBackground("RightIR", cv::Point(8, 16));
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
} break;
|
||||
}
|
||||
|
||||
// Timestamp information with background
|
||||
putTextWithBackground("frame timestamp(us): " + std::to_string(frame->getTimeStampUs()), cv::Point(8, 40));
|
||||
putTextWithBackground("system timestamp(us): " + std::to_string(frame->getSystemTimeStampUs()), cv::Point(8, 64));
|
||||
}
|
||||
|
||||
cv::Mat CVWindow::resizeMatKeepAspectRatio(const cv::Mat &mat, int width, int height) {
|
||||
auto hScale = static_cast<double>(width) / mat.cols;
|
||||
auto vScale = static_cast<double>(height) / mat.rows;
|
||||
auto scale = std::min(hScale, vScale);
|
||||
auto newWidth = static_cast<int>(mat.cols * scale);
|
||||
auto newHeight = static_cast<int>(mat.rows * scale);
|
||||
cv::Mat resizeMat;
|
||||
cv::resize(mat, resizeMat, cv::Size(newWidth, newHeight));
|
||||
|
||||
if(newWidth == width && newHeight == height) {
|
||||
return resizeMat;
|
||||
}
|
||||
|
||||
// padding the resized mat to target width and height
|
||||
cv::Mat paddedMat;
|
||||
if(newWidth < width) {
|
||||
auto paddingLeft = (width - newWidth) / 2;
|
||||
auto paddingRight = width - newWidth - paddingLeft;
|
||||
cv::copyMakeBorder(resizeMat, paddedMat, 0, 0, paddingLeft, paddingRight, cv::BORDER_CONSTANT, cv::Scalar(0, 0, 0));
|
||||
}
|
||||
|
||||
if(newHeight < height) {
|
||||
auto paddingTop = (height - newHeight) / 2;
|
||||
auto paddingBottom = height - newHeight - paddingTop;
|
||||
cv::copyMakeBorder(resizeMat, paddedMat, paddingTop, paddingBottom, 0, 0, cv::BORDER_CONSTANT, cv::Scalar(0, 0, 0));
|
||||
}
|
||||
return paddedMat;
|
||||
}
|
||||
|
||||
} // namespace ob_smpl
|
||||
Reference in New Issue
Block a user