Chiebot-Mirror
/
opencv
mirror of https://github.com/opencv/opencv.git
8
0
Fork 0
Code Issues Projects Releases Wiki Activity

Merge pull request #22936 from hzcyf:orbbec_new_cam_support

Browse Source 
videoio: add Orbbec Gemini 2 and Astra 2 camera support

### Test Result

| OS | Compiler | Camera | Result |
|-----|-----------|---------|--------|
|Windows11| (VS2022)MSVC17.3|Orbbec Gemini 2|Pass|
|Windows11| (VS2022)MSVC17.3|Orbbec Astra 2|Pass|
|Ubuntu22.04|GCC9.2|Orbbec Gemini 2|Pass|
|Ubuntu22.04|GCC9.2|Orbbec Astra 2|Pass|

### Pull Request Readiness Checklist
- [x] I agree to contribute to the project under Apache 2 License.
- [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV
- [x] The PR is proposed to the proper branch
- [x] The feature is well documented and sample code can be built with the project CMake
pull/22979/head
hzc 2 years ago committed by GitHub
parent 50da209dc4
commit 47fb79bd8c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 211 additions and 28 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 7
      modules/videoio/src/cap_obsensor/obsensor_stream_channel_interface.hpp
  2. 6
      modules/videoio/src/cap_obsensor/obsensor_stream_channel_msmf.cpp
  3. 154
      modules/videoio/src/cap_obsensor/obsensor_uvc_stream_channel.cpp
  4. 28
      modules/videoio/src/cap_obsensor/obsensor_uvc_stream_channel.hpp
  5. 40
      modules/videoio/src/cap_obsensor_capture.cpp
  6. 4
      modules/videoio/src/cap_obsensor_capture.hpp

7
modules/videoio/src/cap_obsensor/obsensor_stream_channel_interface.hpp
Unescape View File

@ -32,6 +32,11 @@
namespace cv { namespace cv {
namespace obsensor { 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
enum StreamType enum StreamType
{ {
OBSENSOR_STREAM_IR = 1, OBSENSOR_STREAM_IR = 1,
@ -45,6 +50,7 @@ enum FrameFormat
FRAME_FORMAT_YUYV = 0, FRAME_FORMAT_YUYV = 0,
FRAME_FORMAT_MJPG = 5, FRAME_FORMAT_MJPG = 5,
FRAME_FORMAT_Y16 = 8, FRAME_FORMAT_Y16 = 8,
FRAME_FORMAT_Y14 = 9,
}; };
enum PropertyId enum PropertyId
@ -93,6 +99,7 @@ public:
virtual bool getProperty(int propId, uint8_t* recvData, uint32_t* recvDataSize) = 0; virtual bool getProperty(int propId, uint8_t* recvData, uint32_t* recvDataSize) = 0;
virtual StreamType streamType() const = 0; virtual StreamType streamType() const = 0;
virtual uint16_t getPid() const =0;
}; };
// "StreamChannelGroup" mean a group of stream channels from same one physical device // "StreamChannelGroup" mean a group of stream channels from same one physical device

6
modules/videoio/src/cap_obsensor/obsensor_stream_channel_msmf.cpp
Unescape View File

@ -474,13 +474,13 @@ STDMETHODIMP MSMFStreamChannel::OnReadSample(HRESULT hrStatus, DWORD dwStreamInd
if (sample) if (sample)
{ {
ComPtr<IMFMediaBuffer> buffer = nullptr; ComPtr<IMFMediaBuffer> buffer = nullptr;
DWORD max_length, current_length; DWORD maxLength, currentLength;
byte* byte_buffer = nullptr; byte* byte_buffer = nullptr;
HR_FAILED_EXEC(sample->GetBufferByIndex(0, &buffer), { return S_OK; }); HR_FAILED_EXEC(sample->GetBufferByIndex(0, &buffer), { return S_OK; });
buffer->Lock(&byte_buffer, &max_length, &current_length); buffer->Lock(&byte_buffer, &maxLength, &currentLength);
Frame fo = { currentProfile_.format, currentProfile_.width, currentProfile_.height, current_length, (uint8_t*)byte_buffer }; Frame fo = { currentProfile_.format, currentProfile_.width, currentProfile_.height, currentLength, (uint8_t*)byte_buffer };
if (depthFrameProcessor_) if (depthFrameProcessor_)
{ {
depthFrameProcessor_->process(&fo); depthFrameProcessor_->process(&fo);

154
modules/videoio/src/cap_obsensor/obsensor_uvc_stream_channel.cpp
Unescape View File

@ -41,6 +41,11 @@ const uint8_t OB_EXT_CMD2[16] = { 0x47, 0x4d, 0x04, 0x00, 0x02, 0x00, 0x56, 0x00
const uint8_t OB_EXT_CMD3[16] = { 0x47, 0x4d, 0x04, 0x00, 0x02, 0x00, 0x58, 0x00, 0x2a, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00 }; const uint8_t OB_EXT_CMD3[16] = { 0x47, 0x4d, 0x04, 0x00, 0x02, 0x00, 0x58, 0x00, 0x2a, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00 };
const uint8_t OB_EXT_CMD4[16] = { 0x47, 0x4d, 0x02, 0x00, 0x03, 0x00, 0x60, 0x00, 0xed, 0x03, 0x00, 0x00 }; const uint8_t OB_EXT_CMD4[16] = { 0x47, 0x4d, 0x02, 0x00, 0x03, 0x00, 0x60, 0x00, 0xed, 0x03, 0x00, 0x00 };
const uint8_t OB_EXT_CMD5[16] = { 0x47, 0x4d, 0x02, 0x00, 0x03, 0x00, 0x62, 0x00, 0xe9, 0x03, 0x00, 0x00 }; const uint8_t OB_EXT_CMD5[16] = { 0x47, 0x4d, 0x02, 0x00, 0x03, 0x00, 0x62, 0x00, 0xe9, 0x03, 0x00, 0x00 };
const uint8_t OB_EXT_CMD6[16] = { 0x47, 0x4d, 0x04, 0x00, 0x02, 0x00, 0x7c, 0x00, 0x2a, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00};
const uint8_t OB_EXT_CMD7[16] = { 0x47, 0x4d, 0x04, 0x00, 0x02, 0x00, 0xfe, 0x12, 0x55, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00 };
const uint8_t OB_EXT_CMD8[16] = { 0x47, 0x4d, 0x04, 0x00, 0x02, 0x00, 0xfe, 0x13, 0x3f, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00 };
const uint8_t OB_EXT_CMD9[16] = { 0x47, 0x4d, 0x04, 0x00, 0x02, 0x00, 0xfa, 0x13, 0x4b, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00 };
const uint8_t OB_EXT_CMD10[16] = { 0x47, 0x4d, 0x04, 0x00, 0x02, 0x00, 0xfa, 0x13, 0x3f, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00 };
#if defined(HAVE_OBSENSOR_V4L2) #if defined(HAVE_OBSENSOR_V4L2)
#define fourCc2Int(a, b, c, d) \ #define fourCc2Int(a, b, c, d) \
@ -54,6 +59,7 @@ const std::map<uint32_t, FrameFormat> fourccToOBFormat = {
{fourCc2Int('Y', 'U', 'Y', '2'), FRAME_FORMAT_YUYV}, {fourCc2Int('Y', 'U', 'Y', '2'), FRAME_FORMAT_YUYV},
{fourCc2Int('M', 'J', 'P', 'G'), FRAME_FORMAT_MJPG}, {fourCc2Int('M', 'J', 'P', 'G'), FRAME_FORMAT_MJPG},
{fourCc2Int('Y', '1', '6', ' '), FRAME_FORMAT_Y16}, {fourCc2Int('Y', '1', '6', ' '), FRAME_FORMAT_Y16},
{fourCc2Int('Y', '1', '4', ' '), FRAME_FORMAT_Y14},
}; };
StreamType parseUvcDeviceNameToStreamType(const std::string& devName) StreamType parseUvcDeviceNameToStreamType(const std::string& devName)
@ -187,6 +193,54 @@ void DepthFrameProcessor::process(Frame* frame)
} }
} }
DepthFrameUnpacker::DepthFrameUnpacker(){
outputDataBuf_ = new uint8_t[OUT_DATA_SIZE];
}
DepthFrameUnpacker::~DepthFrameUnpacker(){
delete[] outputDataBuf_;
}
#define ON_BITS(count) ((1 << count) - 1)
#define CREATE_MASK(count, offset) (ON_BITS(count) << offset)
#define TAKE_BITS(source, count, offset) ((source & CREATE_MASK(count, offset)) >> offset)
void DepthFrameUnpacker::process(Frame *frame){
const uint8_t tarStep = 16;
const uint8_t srcStep = 28;
uint16_t *tar = (uint16_t *)outputDataBuf_;
uint8_t *src = frame->data;
uint32_t pixelSize = frame->width * frame->height;
for(uint32_t i = 0; i < pixelSize; i += tarStep) {
tar[0] = (TAKE_BITS(src[0], 8, 0) << 6) | TAKE_BITS(src[1], 6, 2);
tar[1] = (TAKE_BITS(src[1], 2, 0) << 12) | (TAKE_BITS(src[2], 8, 0) << 4) | TAKE_BITS(src[3], 4, 4);
tar[2] = (TAKE_BITS(src[3], 4, 0) << 10) | (TAKE_BITS(src[4], 8, 0) << 2) | TAKE_BITS(src[5], 2, 6);
tar[3] = (TAKE_BITS(src[5], 6, 0) << 8) | TAKE_BITS(src[6], 8, 0);
tar[4] = (TAKE_BITS(src[7], 8, 0) << 6) | TAKE_BITS(src[8], 6, 2);
tar[5] = (TAKE_BITS(src[8], 2, 0) << 12) | (TAKE_BITS(src[9], 8, 0) << 4) | TAKE_BITS(src[10], 4, 4);
tar[6] = (TAKE_BITS(src[10], 4, 0) << 10) | (TAKE_BITS(src[11], 8, 0) << 2) | TAKE_BITS(src[12], 2, 6);
tar[7] = (TAKE_BITS(src[12], 6, 0) << 8) | TAKE_BITS(src[13], 8, 0);
tar[8] = (TAKE_BITS(src[14], 8, 0) << 6) | TAKE_BITS(src[15], 6, 2);
tar[9] = (TAKE_BITS(src[15], 2, 0) << 12) | (TAKE_BITS(src[16], 8, 0) << 4) | TAKE_BITS(src[17], 4, 4);
tar[10] = (TAKE_BITS(src[17], 4, 0) << 10) | (TAKE_BITS(src[18], 8, 0) << 2) | TAKE_BITS(src[19], 2, 6);
tar[11] = (TAKE_BITS(src[19], 6, 0) << 8) | TAKE_BITS(src[20], 8, 0);
tar[12] = (TAKE_BITS(src[21], 8, 0) << 6) | TAKE_BITS(src[22], 6, 2);
tar[13] = (TAKE_BITS(src[22], 2, 0) << 12) | (TAKE_BITS(src[23], 8, 0) << 4) | TAKE_BITS(src[24], 4, 4);
tar[14] = (TAKE_BITS(src[24], 4, 0) << 10) | (TAKE_BITS(src[25], 8, 0) << 2) | TAKE_BITS(src[26], 2, 6);
tar[15] = (TAKE_BITS(src[26], 6, 0) << 8) | TAKE_BITS(src[27], 8, 0);
src += srcStep;
tar += tarStep;
}
frame->data = outputDataBuf_;
frame->format = FRAME_FORMAT_Y16;
}
IUvcStreamChannel::IUvcStreamChannel(const UvcDeviceInfo& devInfo) : IUvcStreamChannel::IUvcStreamChannel(const UvcDeviceInfo& devInfo) :
devInfo_(devInfo), devInfo_(devInfo),
streamType_(parseUvcDeviceNameToStreamType(devInfo_.name)) streamType_(parseUvcDeviceNameToStreamType(devInfo_.name))
@ -198,6 +252,10 @@ StreamType IUvcStreamChannel::streamType() const {
return streamType_; return streamType_;
} }
uint16_t IUvcStreamChannel::getPid() const {
return devInfo_.pid;
};
bool IUvcStreamChannel::setProperty(int propId, const uint8_t* /*data*/, uint32_t /*dataSize*/) bool IUvcStreamChannel::setProperty(int propId, const uint8_t* /*data*/, uint32_t /*dataSize*/)
{ {
uint8_t* rcvData; uint8_t* rcvData;
@ -206,15 +264,28 @@ bool IUvcStreamChannel::setProperty(int propId, const uint8_t* /*data*/, uint32_
switch (propId) switch (propId)
{ {
case DEPTH_TO_COLOR_ALIGN: case DEPTH_TO_COLOR_ALIGN:
// todo: value filling if(OBSENSOR_GEMINI2_PID == devInfo_.pid ){
rst &= setXu(2, OB_EXT_CMD0, sizeof(OB_EXT_CMD0)); rst &= setXu(2, OB_EXT_CMD8, sizeof(OB_EXT_CMD8));
rst &= getXu(2, &rcvData, &rcvLen); rst &= getXu(2, &rcvData, &rcvLen);
rst &= setXu(2, OB_EXT_CMD1, sizeof(OB_EXT_CMD1)); rst &= setXu(2, OB_EXT_CMD6, sizeof(OB_EXT_CMD6));
rst &= getXu(2, &rcvData, &rcvLen); rst &= getXu(2, &rcvData, &rcvLen);
rst &= setXu(2, OB_EXT_CMD2, sizeof(OB_EXT_CMD2)); }
rst &= getXu(2, &rcvData, &rcvLen); else if(OBSENSOR_ASTRA2_PID == devInfo_.pid ){
rst &= setXu(2, OB_EXT_CMD3, sizeof(OB_EXT_CMD3)); rst &= setXu(2, OB_EXT_CMD10, sizeof(OB_EXT_CMD8));
rst &= getXu(2, &rcvData, &rcvLen); rst &= getXu(2, &rcvData, &rcvLen);
rst &= setXu(2, OB_EXT_CMD6, sizeof(OB_EXT_CMD6));
rst &= getXu(2, &rcvData, &rcvLen);
}
else{
rst &= setXu(2, OB_EXT_CMD0, sizeof(OB_EXT_CMD0));
rst &= getXu(2, &rcvData, &rcvLen);
rst &= setXu(2, OB_EXT_CMD1, sizeof(OB_EXT_CMD1));
rst &= getXu(2, &rcvData, &rcvLen);
rst &= setXu(2, OB_EXT_CMD2, sizeof(OB_EXT_CMD2));
rst &= getXu(2, &rcvData, &rcvLen);
rst &= setXu(2, OB_EXT_CMD3, sizeof(OB_EXT_CMD3));
rst &= getXu(2, &rcvData, &rcvLen);
}
break; break;
default: default:
rst = false; rst = false;
@ -231,12 +302,50 @@ bool IUvcStreamChannel::getProperty(int propId, uint8_t* recvData, uint32_t* rec
switch (propId) switch (propId)
{ {
case CAMERA_PARAM: case CAMERA_PARAM:
rst &= setXu(2, OB_EXT_CMD5, sizeof(OB_EXT_CMD5)); if(OBSENSOR_GEMINI2_PID == devInfo_.pid){
rst &= getXu(2, &rcvData, &rcvLen); // return default param
if (rst && OB_EXT_CMD5[6] == rcvData[6] && rcvData[8] == 0 && rcvData[9] == 0) CameraParam param;
{ param.p0[0] = 516.652f;
memcpy(recvData, rcvData + 10, rcvLen - 10); param.p0[1] = 516.692f;
*recvDataSize = rcvLen - 10; param.p0[2] = 322.988f;
param.p0[3] = 235.787f;
param.p1[0] = 516.652f;
param.p1[1] = 516.692f;
param.p1[2] = 322.988f;
param.p1[3] = 235.787f;
param.p6[0] = 640;
param.p6[1] = 480;
param.p7[0] = 640;
param.p7[1] = 480;
*recvDataSize = sizeof(CameraParam);
memcpy(recvData, &param, *recvDataSize);
}
else if(OBSENSOR_ASTRA2_PID == devInfo_.pid){
// return default param
CameraParam param;
param.p0[0] = 558.151f;
param.p0[1] = 558.003f;
param.p0[2] = 312.546f;
param.p0[3] = 241.169f;
param.p1[0] = 558.151f;
param.p1[1] = 558.003f;
param.p1[2] = 312.546f;
param.p1[3] = 241.169f;
param.p6[0] = 640;
param.p6[1] = 480;
param.p7[0] = 640;
param.p7[1] = 480;
*recvDataSize = sizeof(CameraParam);
memcpy(recvData, &param, *recvDataSize);
}
else{
rst &= setXu(2, OB_EXT_CMD5, sizeof(OB_EXT_CMD5));
rst &= getXu(2, &rcvData, &rcvLen);
if (rst && OB_EXT_CMD5[6] == rcvData[6] && rcvData[8] == 0 && rcvData[9] == 0)
{
memcpy(recvData, rcvData + 10, rcvLen - 10);
*recvDataSize = rcvLen - 10;
}
} }
break; break;
default: default:
@ -249,7 +358,20 @@ bool IUvcStreamChannel::getProperty(int propId, uint8_t* recvData, uint32_t* rec
bool IUvcStreamChannel::initDepthFrameProcessor() bool IUvcStreamChannel::initDepthFrameProcessor()
{ {
if (streamType_ == OBSENSOR_STREAM_DEPTH && setXu(2, OB_EXT_CMD4, sizeof(OB_EXT_CMD4))) if(OBSENSOR_GEMINI2_PID == devInfo_.pid || OBSENSOR_ASTRA2_PID == devInfo_.pid){
uint8_t* rcvData;
uint32_t rcvLen;
setXu(2, OB_EXT_CMD7, sizeof(OB_EXT_CMD7));
getXu(2, &rcvData, &rcvLen);
setXu(2, OB_EXT_CMD9, sizeof(OB_EXT_CMD9));
getXu(2, &rcvData, &rcvLen);
depthFrameProcessor_ = makePtr<DepthFrameUnpacker>();
return true;
}
else if (streamType_ == OBSENSOR_STREAM_DEPTH && setXu(2, OB_EXT_CMD4, sizeof(OB_EXT_CMD4)))
{ {
uint8_t* rcvData; uint8_t* rcvData;
uint32_t rcvLen; uint32_t rcvLen;

28
modules/videoio/src/cap_obsensor/obsensor_uvc_stream_channel.hpp
Unescape View File

@ -26,8 +26,6 @@
#ifdef HAVE_OBSENSOR #ifdef HAVE_OBSENSOR
namespace cv { namespace cv {
namespace obsensor { namespace obsensor {
#define OBSENSOR_CAM_VID 0x2bc5 // usb vid
#define XU_MAX_DATA_LENGTH 1024 #define XU_MAX_DATA_LENGTH 1024
#define XU_UNIT_ID 4 #define XU_UNIT_ID 4
@ -60,17 +58,34 @@ struct OBExtensionParam {
float ps; float ps;
}; };
class DepthFrameProcessor { class IFrameProcessor{
public:
virtual void process(Frame* frame) = 0;
virtual ~IFrameProcessor() = default;
};
class DepthFrameProcessor: public IFrameProcessor {
public: public:
DepthFrameProcessor(const OBExtensionParam& parma); DepthFrameProcessor(const OBExtensionParam& parma);
~DepthFrameProcessor() noexcept; virtual ~DepthFrameProcessor() noexcept;
void process(Frame* frame); virtual void process(Frame* frame) override;
private: private:
const OBExtensionParam param_; const OBExtensionParam param_;
uint16_t* lookUpTable_; uint16_t* lookUpTable_;
}; };
class DepthFrameUnpacker: public IFrameProcessor {
public:
DepthFrameUnpacker();
virtual ~DepthFrameUnpacker() noexcept;
virtual void process(Frame* frame) override;
private:
const uint32_t OUT_DATA_SIZE = 1280*800*2;
uint8_t *outputDataBuf_;
};
class IUvcStreamChannel : public IStreamChannel { class IUvcStreamChannel : public IStreamChannel {
public: public:
IUvcStreamChannel(const UvcDeviceInfo& devInfo); IUvcStreamChannel(const UvcDeviceInfo& devInfo);
@ -79,6 +94,7 @@ public:
virtual bool setProperty(int propId, const uint8_t* data, uint32_t dataSize) override; virtual bool setProperty(int propId, const uint8_t* data, uint32_t dataSize) override;
virtual bool getProperty(int propId, uint8_t* recvData, uint32_t* recvDataSize) override; virtual bool getProperty(int propId, uint8_t* recvData, uint32_t* recvDataSize) override;
virtual StreamType streamType() const override; virtual StreamType streamType() const override;
virtual uint16_t getPid() const override;
protected: protected:
virtual bool setXu(uint8_t ctrl, const uint8_t* data, uint32_t len) = 0; virtual bool setXu(uint8_t ctrl, const uint8_t* data, uint32_t len) = 0;
@ -89,7 +105,7 @@ protected:
protected: protected:
const UvcDeviceInfo devInfo_; const UvcDeviceInfo devInfo_;
StreamType streamType_; StreamType streamType_;
Ptr<DepthFrameProcessor> depthFrameProcessor_; Ptr<IFrameProcessor> depthFrameProcessor_;
}; };
}} // namespace cv::obsensor:: }} // namespace cv::obsensor::
#endif // HAVE_OBSENSOR #endif // HAVE_OBSENSOR

40
modules/videoio/src/cap_obsensor_capture.cpp
Unescape View File

@ -34,6 +34,8 @@ VideoCapture_obsensor::VideoCapture_obsensor(int index) : isOpened_(false)
{ {
static const obsensor::StreamProfile colorProfile = { 640, 480, 30, obsensor::FRAME_FORMAT_MJPG }; 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 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};
streamChannelGroup_ = obsensor::getStreamChannelGroup(index); streamChannelGroup_ = obsensor::getStreamChannelGroup(index);
if (!streamChannelGroup_.empty()) if (!streamChannelGroup_.empty())
@ -47,15 +49,27 @@ VideoCapture_obsensor::VideoCapture_obsensor(int index) : isOpened_(false)
channel->start(colorProfile, [&](obsensor::Frame* frame) { channel->start(colorProfile, [&](obsensor::Frame* frame) {
std::unique_lock<std::mutex> lk(frameMutex_); std::unique_lock<std::mutex> lk(frameMutex_);
colorFrame_ = Mat(1, frame->dataSize, CV_8UC1, frame->data).clone(); colorFrame_ = Mat(1, frame->dataSize, CV_8UC1, frame->data).clone();
frameCv_.notify_all();
}); });
break; break;
case obsensor::OBSENSOR_STREAM_DEPTH: case obsensor::OBSENSOR_STREAM_DEPTH:
{ {
uint8_t data = 1; uint8_t data = 1;
channel->setProperty(obsensor::DEPTH_TO_COLOR_ALIGN, &data, 1); channel->setProperty(obsensor::DEPTH_TO_COLOR_ALIGN, &data, 1);
channel->start(depthProfile, [&](obsensor::Frame* frame) {
obsensor::StreamProfile profile = depthProfile;
if(OBSENSOR_GEMINI2_PID == channel->getPid()){
profile = gemini2depthProfile;
}
else if(OBSENSOR_ASTRA2_PID == channel->getPid()){
profile = astra2depthProfile;
}
channel->start(profile, [&](obsensor::Frame* frame) {
std::unique_lock<std::mutex> lk(frameMutex_); std::unique_lock<std::mutex> lk(frameMutex_);
depthFrame_ = Mat(frame->height, frame->width, CV_16UC1, frame->data, frame->width * 2).clone(); depthFrame_ = Mat(frame->height, frame->width, CV_16UC1, frame->data, frame->width * 2).clone();
frameCv_.notify_all();
}); });
uint32_t len; uint32_t len;
@ -72,10 +86,21 @@ VideoCapture_obsensor::VideoCapture_obsensor(int index) : isOpened_(false)
} }
} }
VideoCapture_obsensor::~VideoCapture_obsensor(){
for (auto& channel : streamChannelGroup_)
{
channel->stop();
}
streamChannelGroup_.clear();
}
bool VideoCapture_obsensor::grabFrame() bool VideoCapture_obsensor::grabFrame()
{ {
std::unique_lock<std::mutex> lk(frameMutex_); std::unique_lock<std::mutex> lk(frameMutex_);
// Try waiting for 33 milliseconds to ensure that both depth and color frame have been received!
frameCv_.wait_for(lk, std::chrono::milliseconds(33), [&](){ return !depthFrame_.empty() && !colorFrame_.empty(); });
grabbedDepthFrame_ = depthFrame_; grabbedDepthFrame_ = depthFrame_;
grabbedColorFrame_ = colorFrame_; grabbedColorFrame_ = colorFrame_;
@ -93,7 +118,18 @@ bool VideoCapture_obsensor::retrieveFrame(int outputType, OutputArray frame)
case CAP_OBSENSOR_DEPTH_MAP: case CAP_OBSENSOR_DEPTH_MAP:
if (!grabbedDepthFrame_.empty()) if (!grabbedDepthFrame_.empty())
{ {
grabbedDepthFrame_.copyTo(frame); if(OBSENSOR_GEMINI2_PID == streamChannelGroup_.front()->getPid()){
grabbedDepthFrame_ = grabbedDepthFrame_*0.8;
Rect rect(320, 160, 640, 480);
grabbedDepthFrame_(rect).copyTo(frame);
}
else if(OBSENSOR_ASTRA2_PID == streamChannelGroup_.front()->getPid()){
grabbedDepthFrame_ = grabbedDepthFrame_*0.8;
grabbedDepthFrame_.copyTo(frame);
}
else{
grabbedDepthFrame_.copyTo(frame);
}
grabbedDepthFrame_.release(); grabbedDepthFrame_.release();
return true; return true;
} }

4
modules/videoio/src/cap_obsensor_capture.hpp
Unescape View File

@ -24,6 +24,7 @@
#include <map> #include <map>
#include <mutex> #include <mutex>
#include <condition_variable>
#include "cap_obsensor/obsensor_stream_channel_interface.hpp" #include "cap_obsensor/obsensor_stream_channel_interface.hpp"
@ -33,7 +34,7 @@ class VideoCapture_obsensor : public IVideoCapture
{ {
public: public:
VideoCapture_obsensor(int index); VideoCapture_obsensor(int index);
virtual ~VideoCapture_obsensor() {} virtual ~VideoCapture_obsensor();
virtual double getProperty(int propIdx) const CV_OVERRIDE; virtual double getProperty(int propIdx) const CV_OVERRIDE;
virtual bool setProperty(int propIdx, double propVal) CV_OVERRIDE; virtual bool setProperty(int propIdx, double propVal) CV_OVERRIDE;
@ -51,6 +52,7 @@ private:
std::vector<Ptr<obsensor::IStreamChannel>> streamChannelGroup_; std::vector<Ptr<obsensor::IStreamChannel>> streamChannelGroup_;
std::mutex frameMutex_; std::mutex frameMutex_;
std::condition_variable frameCv_;
Mat depthFrame_; Mat depthFrame_;
Mat colorFrame_; Mat colorFrame_;

Write Preview
Loading…
Cancel
Save