From 325fe7e663daa7dfe29a5213beb7b1b0598923db Mon Sep 17 00:00:00 2001 From: hzcyf Date: Sat, 11 Feb 2023 14:11:40 +0800 Subject: [PATCH] add support for Orbbec Femto Mega RGB-D camera --- .../obsensor_stream_channel_interface.hpp | 1 + .../obsensor_stream_channel_msmf.cpp | 2 +- .../obsensor_uvc_stream_channel.cpp | 18 +++++++++++++ modules/videoio/src/cap_obsensor_capture.cpp | 26 ++++++++++++++----- 4 files changed, 40 insertions(+), 7 deletions(-) diff --git a/modules/videoio/src/cap_obsensor/obsensor_stream_channel_interface.hpp b/modules/videoio/src/cap_obsensor/obsensor_stream_channel_interface.hpp index ac0cf1259edf..ff78c5a69646 100644 --- a/modules/videoio/src/cap_obsensor/obsensor_stream_channel_interface.hpp +++ b/modules/videoio/src/cap_obsensor/obsensor_stream_channel_interface.hpp @@ -36,6 +36,7 @@ namespace obsensor { #define OBSENSOR_CAM_VID 0x2bc5 // usb vid #define OBSENSOR_ASTRA2_PID 0x0660 // pid of Orbbec Astra 2 Camera #define OBSENSOR_GEMINI2_PID 0x0670 // pid of Orbbec Gemini 2 Camera +#define OBSENSOR_FEMTO_MEGA_PID 0x0669 // pid of Orbbec Femto Mega Camera enum StreamType { diff --git a/modules/videoio/src/cap_obsensor/obsensor_stream_channel_msmf.cpp b/modules/videoio/src/cap_obsensor/obsensor_stream_channel_msmf.cpp index 7d984b63de4f..5de686430f7c 100644 --- a/modules/videoio/src/cap_obsensor/obsensor_stream_channel_msmf.cpp +++ b/modules/videoio/src/cap_obsensor/obsensor_stream_channel_msmf.cpp @@ -499,7 +499,7 @@ STDMETHODIMP MSMFStreamChannel::OnEvent(DWORD /*sidx*/, IMFMediaEvent* /*event*/ STDMETHODIMP MSMFStreamChannel::OnFlush(DWORD) { - if (streamState_ == STREAM_STARTING) + if (streamState_ != STREAM_STOPED) { std::unique_lock lock(streamStateMutex_); streamState_ = STREAM_STOPED; diff --git a/modules/videoio/src/cap_obsensor/obsensor_uvc_stream_channel.cpp b/modules/videoio/src/cap_obsensor/obsensor_uvc_stream_channel.cpp index 1bcb6ddf766d..55c2d03fd731 100644 --- a/modules/videoio/src/cap_obsensor/obsensor_uvc_stream_channel.cpp +++ b/modules/videoio/src/cap_obsensor/obsensor_uvc_stream_channel.cpp @@ -338,6 +338,24 @@ bool IUvcStreamChannel::getProperty(int propId, uint8_t* recvData, uint32_t* rec *recvDataSize = sizeof(CameraParam); memcpy(recvData, ¶m, *recvDataSize); } + else if(OBSENSOR_FEMTO_MEGA_PID == devInfo_.pid){ + // return default param + CameraParam param; + param.p0[0] = 748.370f; + param.p0[1] = 748.296f; + param.p0[2] = 634.670f; + param.p0[3] = 341.196f; + param.p1[0] = 374.185f; + param.p1[1] = 374.148f; + param.p1[2] = 317.335f; + param.p1[3] = 170.598f; + param.p6[0] = 1280; + param.p6[1] = 720; + param.p7[0] = 640; + param.p7[1] = 360; + *recvDataSize = sizeof(CameraParam); + memcpy(recvData, ¶m, *recvDataSize); + } else{ rst &= setXu(2, OB_EXT_CMD5, sizeof(OB_EXT_CMD5)); rst &= getXu(2, &rcvData, &rcvLen); diff --git a/modules/videoio/src/cap_obsensor_capture.cpp b/modules/videoio/src/cap_obsensor_capture.cpp index ccbfd61a5cf7..8138f0933310 100644 --- a/modules/videoio/src/cap_obsensor_capture.cpp +++ b/modules/videoio/src/cap_obsensor_capture.cpp @@ -34,8 +34,10 @@ VideoCapture_obsensor::VideoCapture_obsensor(int index) : isOpened_(false) { static const obsensor::StreamProfile colorProfile = { 640, 480, 30, obsensor::FRAME_FORMAT_MJPG }; static const obsensor::StreamProfile depthProfile = {640, 480, 30, obsensor::FRAME_FORMAT_Y16}; - static const obsensor::StreamProfile gemini2depthProfile = {1280, 800, 30, obsensor::FRAME_FORMAT_Y14}; - static const obsensor::StreamProfile astra2depthProfile = {640, 480, 30, obsensor::FRAME_FORMAT_Y14}; + static const obsensor::StreamProfile gemini2DepthProfile = {1280, 800, 30, obsensor::FRAME_FORMAT_Y14}; + static const obsensor::StreamProfile astra2DepthProfile = {640, 480, 30, obsensor::FRAME_FORMAT_Y14}; + static const obsensor::StreamProfile megaColorProfile = {1280, 720, 30, obsensor::FRAME_FORMAT_MJPG}; + static const obsensor::StreamProfile megaDepthProfile = {640, 576, 30, obsensor::FRAME_FORMAT_Y16}; streamChannelGroup_ = obsensor::getStreamChannelGroup(index); if (!streamChannelGroup_.empty()) @@ -46,11 +48,17 @@ VideoCapture_obsensor::VideoCapture_obsensor(int index) : isOpened_(false) switch (streamType) { case obsensor::OBSENSOR_STREAM_COLOR: - channel->start(colorProfile, [&](obsensor::Frame* frame) { + { + auto profile = colorProfile; + if(OBSENSOR_FEMTO_MEGA_PID == channel->getPid()){ + profile = megaColorProfile; + } + channel->start(profile, [&](obsensor::Frame* frame) { std::unique_lock lk(frameMutex_); colorFrame_ = Mat(1, frame->dataSize, CV_8UC1, frame->data).clone(); frameCv_.notify_all(); }); + } break; case obsensor::OBSENSOR_STREAM_DEPTH: { @@ -59,11 +67,13 @@ VideoCapture_obsensor::VideoCapture_obsensor(int index) : isOpened_(false) obsensor::StreamProfile profile = depthProfile; if(OBSENSOR_GEMINI2_PID == channel->getPid()){ - profile = gemini2depthProfile; + profile = gemini2DepthProfile; } else if(OBSENSOR_ASTRA2_PID == channel->getPid()){ - - profile = astra2depthProfile; + profile = astra2DepthProfile; + } + else if(OBSENSOR_FEMTO_MEGA_PID == channel->getPid()){ + profile = megaDepthProfile; } channel->start(profile, [&](obsensor::Frame* frame) { @@ -127,6 +137,10 @@ bool VideoCapture_obsensor::retrieveFrame(int outputType, OutputArray frame) grabbedDepthFrame_ = grabbedDepthFrame_*0.8; grabbedDepthFrame_.copyTo(frame); } + else if(OBSENSOR_FEMTO_MEGA_PID == streamChannelGroup_.front()->getPid()){ + Rect rect(0, 0, 640, 360); + grabbedDepthFrame_(rect).copyTo(frame); + } else{ grabbedDepthFrame_.copyTo(frame); }