mirror of https://github.com/opencv/opencv.git
videoio: add support for obsensor (Orbbec RGB-D Camera ) (#22196)
* videoio: add support for obsensor (Orbbec RGB-D Camera ) * obsensor: code format issues fixed and some code optimized * obsensor: fix typo and format issues * obsensor: fix crosses initialization errorpull/22313/head
hzc
2 years ago
committed by
GitHub
parent
ebaf8cc06c
commit
fc3e393516
17 changed files with 1992 additions and 1 deletions
@ -0,0 +1,18 @@ |
||||
# --- obsensor --- |
||||
if(NOT HAVE_OBSENSOR) |
||||
if(WIN32) |
||||
check_include_file(mfapi.h HAVE_MFAPI) |
||||
if(HAVE_MFAPI) |
||||
set(HAVE_OBSENSOR TRUE) |
||||
set(HAVE_OBSENSOR_MSMF TRUE) |
||||
ocv_add_external_target(obsensor "" "" "HAVE_OBSENSOR;HAVE_OBSENSOR_MSMF") |
||||
endif() |
||||
elseif(UNIX) |
||||
check_include_file(linux/videodev2.h HAVE_CAMV4L2) |
||||
if(HAVE_CAMV4L2) |
||||
set(HAVE_OBSENSOR TRUE) |
||||
set(HAVE_OBSENSOR_V4L2 TRUE) |
||||
ocv_add_external_target(obsensor "" "" "HAVE_OBSENSOR;HAVE_OBSENSOR_V4L2") |
||||
endif() |
||||
endif() |
||||
endif() |
||||
@ -0,0 +1,103 @@ |
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html.
|
||||
|
||||
/*
|
||||
* Copyright(C) 2022 by ORBBEC Technology., Inc. |
||||
* Authors: |
||||
* Huang Zhenchang <yufeng@orbbec.com> |
||||
* |
||||
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||
* you may not use this file except in compliance with the License. |
||||
* You may obtain a copy of the License at |
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
* |
||||
* Unless required by applicable law or agreed to in writing, software |
||||
* distributed under the License is distributed on an "AS IS" BASIS, |
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||
* See the License for the specific language governing permissions and |
||||
* limitations under the License. |
||||
*/ |
||||
|
||||
#ifndef OPENCV_VIDEOIO_OBSENSOR_STREAM_CHANNEL_INTERFACE_HPP |
||||
#define OPENCV_VIDEOIO_OBSENSOR_STREAM_CHANNEL_INTERFACE_HPP |
||||
|
||||
#ifdef HAVE_OBSENSOR |
||||
|
||||
#include "../precomp.hpp" // #include "precomp.hpp : compile error on linux |
||||
|
||||
#include <functional> |
||||
#include <vector> |
||||
#include <memory> |
||||
|
||||
namespace cv { |
||||
namespace obsensor { |
||||
enum StreamType |
||||
{ |
||||
OBSENSOR_STREAM_IR = 1, |
||||
OBSENSOR_STREAM_COLOR = 2, |
||||
OBSENSOR_STREAM_DEPTH = 3, |
||||
}; |
||||
|
||||
enum FrameFormat |
||||
{ |
||||
FRAME_FORMAT_UNKNOWN = -1, |
||||
FRAME_FORMAT_YUYV = 0, |
||||
FRAME_FORMAT_MJPG = 5, |
||||
FRAME_FORMAT_Y16 = 8, |
||||
}; |
||||
|
||||
enum PropertyId |
||||
{ |
||||
DEPTH_TO_COLOR_ALIGN = 42, |
||||
CAMERA_PARAM = 1001, |
||||
}; |
||||
|
||||
struct Frame |
||||
{ |
||||
FrameFormat format; |
||||
uint32_t width; |
||||
uint32_t height; |
||||
uint32_t dataSize; |
||||
uint8_t* data; |
||||
}; |
||||
|
||||
struct StreamProfile |
||||
{ |
||||
uint32_t width; |
||||
uint32_t height; |
||||
uint32_t fps; |
||||
FrameFormat format; |
||||
}; |
||||
|
||||
struct CameraParam |
||||
{ |
||||
float p0[4]; |
||||
float p1[4]; |
||||
float p2[9]; |
||||
float p3[3]; |
||||
float p4[5]; |
||||
float p5[5]; |
||||
uint32_t p6[2]; |
||||
uint32_t p7[2]; |
||||
}; |
||||
|
||||
typedef std::function<void(Frame*)> FrameCallback; |
||||
class IStreamChannel |
||||
{ |
||||
public: |
||||
virtual ~IStreamChannel() noexcept {} |
||||
virtual void start(const StreamProfile& profile, FrameCallback frameCallback) = 0; |
||||
virtual void stop() = 0; |
||||
virtual bool setProperty(int propId, const uint8_t* data, uint32_t dataSize) = 0; |
||||
virtual bool getProperty(int propId, uint8_t* recvData, uint32_t* recvDataSize) = 0; |
||||
|
||||
virtual StreamType streamType() const = 0; |
||||
}; |
||||
|
||||
// "StreamChannelGroup" mean a group of stream channels from same one physical device
|
||||
std::vector<Ptr<IStreamChannel>> getStreamChannelGroup(uint32_t groupIdx = 0); |
||||
|
||||
}} // namespace cv::obsensor::
|
||||
#endif // HAVE_OBSENSOR
|
||||
#endif // OPENCV_VIDEOIO_OBSENSOR_STREAM_CHANNEL_INTERFACE_HPP
|
||||
@ -0,0 +1,505 @@ |
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html.
|
||||
|
||||
/*
|
||||
* Copyright(C) 2022 by ORBBEC Technology., Inc. |
||||
* Authors: |
||||
* Huang Zhenchang <yufeng@orbbec.com> |
||||
* |
||||
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||
* you may not use this file except in compliance with the License. |
||||
* You may obtain a copy of the License at |
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
* |
||||
* Unless required by applicable law or agreed to in writing, software |
||||
* distributed under the License is distributed on an "AS IS" BASIS, |
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||
* See the License for the specific language governing permissions and |
||||
* limitations under the License. |
||||
*/ |
||||
|
||||
|
||||
#ifdef HAVE_OBSENSOR_MSMF |
||||
|
||||
#include "obsensor_stream_channel_msmf.hpp" |
||||
|
||||
#include <shlwapi.h> // QISearch |
||||
|
||||
#pragma warning(disable : 4503) |
||||
#pragma comment(lib, "mfplat") |
||||
#pragma comment(lib, "mf") |
||||
#pragma comment(lib, "mfuuid") |
||||
#pragma comment(lib, "Strmiids") |
||||
#pragma comment(lib, "Mfreadwrite") |
||||
#pragma comment(lib, "dxgi") |
||||
|
||||
namespace cv { |
||||
namespace obsensor { |
||||
std::string wideCharToUTF8(const WCHAR* s) |
||||
{ |
||||
auto len = WideCharToMultiByte(CP_UTF8, 0, s, -1, nullptr, 0, nullptr, nullptr); |
||||
if (len == 0) |
||||
return ""; |
||||
std::string buffer(len - 1, ' '); |
||||
len = WideCharToMultiByte(CP_UTF8, 0, s, -1, &buffer[0], static_cast<int>(buffer.size()) + 1, nullptr, nullptr); |
||||
return buffer; |
||||
} |
||||
|
||||
std::string hr_to_string(HRESULT hr) |
||||
{ |
||||
_com_error err(hr); |
||||
std::stringstream ss; |
||||
ss << "HResult 0x" << std::hex << hr << ": \"" << err.ErrorMessage() << "\""; |
||||
return ss.str(); |
||||
} |
||||
|
||||
#define HR_FAILED_RETURN(x) \ |
||||
if (x < 0) \
|
||||
{ \
|
||||
CV_LOG_INFO(NULL, "Media Foundation error return: " << hr_to_string(x)); \
|
||||
return; \
|
||||
} |
||||
|
||||
#define HR_FAILED_LOG(x) \ |
||||
if (x < 0) \
|
||||
{ \
|
||||
CV_LOG_INFO(NULL, "Media Foundation error return: " << hr_to_string(x)); \
|
||||
} |
||||
|
||||
#define HR_FAILED_EXEC(x, statement) \ |
||||
if (x < 0) \
|
||||
{ \
|
||||
CV_LOG_INFO(NULL, "Media Foundation error return: " << hr_to_string(x)); \
|
||||
statement; \
|
||||
} |
||||
|
||||
std::vector<std::string> stringSplit(std::string string, char separator) |
||||
{ |
||||
std::vector<std::string> tokens; |
||||
std::string::size_type i1 = 0; |
||||
while (true) |
||||
{ |
||||
auto i2 = string.find(separator, i1); |
||||
if (i2 == std::string::npos) |
||||
{ |
||||
tokens.push_back(string.substr(i1)); |
||||
return tokens; |
||||
} |
||||
tokens.push_back(string.substr(i1, i2 - i1)); |
||||
i1 = i2 + 1; |
||||
} |
||||
} |
||||
|
||||
bool parseUvcDeviceSymbolicLink(const std::string& symbolicLink, uint16_t& vid, uint16_t& pid, uint16_t& mi, std::string& unique_id, |
||||
std::string& device_guid) |
||||
{ |
||||
std::string lowerStr = symbolicLink; |
||||
for (size_t i = 0; i < lowerStr.length(); i++) |
||||
{ |
||||
lowerStr[i] = (char)tolower(lowerStr[i]);
|
||||
} |
||||
auto tokens = stringSplit(lowerStr, '#'); |
||||
if (tokens.size() < 1 || (tokens[0] != R"(\\?\usb)" && tokens[0] != R"(\\?\hid)")) |
||||
return false; // Not a USB device
|
||||
if (tokens.size() < 3) |
||||
{ |
||||
return false; |
||||
} |
||||
|
||||
auto ids = stringSplit(tokens[1], '&'); |
||||
if (ids[0].size() != 8 || ids[0].substr(0, 4) != "vid_" || !(std::istringstream(ids[0].substr(4, 4)) >> std::hex >> vid)) |
||||
{ |
||||
return false; |
||||
} |
||||
if (ids[1].size() != 8 || ids[1].substr(0, 4) != "pid_" || !(std::istringstream(ids[1].substr(4, 4)) >> std::hex >> pid)) |
||||
{ |
||||
return false; |
||||
} |
||||
if (ids.size() > 2 && (ids[2].size() != 5 || ids[2].substr(0, 3) != "mi_" || !(std::istringstream(ids[2].substr(3, 2)) >> mi))) |
||||
{ |
||||
return false; |
||||
} |
||||
ids = stringSplit(tokens[2], '&'); |
||||
if (ids.size() == 0) |
||||
{ |
||||
return false; |
||||
} |
||||
|
||||
if (ids.size() > 2) |
||||
unique_id = ids[1]; |
||||
else |
||||
unique_id = ""; |
||||
|
||||
if (tokens.size() >= 3) |
||||
device_guid = tokens[3]; |
||||
|
||||
return true; |
||||
} |
||||
|
||||
#pragma pack(push, 1) |
||||
template <class T> |
||||
class big_endian |
||||
{ |
||||
T be_value; |
||||
|
||||
public: |
||||
operator T() const |
||||
{ |
||||
T le_value = 0; |
||||
for (unsigned int i = 0; i < sizeof(T); ++i) |
||||
reinterpret_cast<char*>(&le_value)[i] = reinterpret_cast<const char*>(&be_value)[sizeof(T) - i - 1]; |
||||
return le_value; |
||||
} |
||||
}; |
||||
#pragma pack(pop) |
||||
|
||||
MFContext::~MFContext(void) |
||||
{ |
||||
CoUninitialize(); |
||||
} |
||||
|
||||
MFContext& MFContext::getInstance() |
||||
{ |
||||
static MFContext instance; |
||||
return instance; |
||||
} |
||||
|
||||
std::vector<UvcDeviceInfo> MFContext::queryUvcDeviceInfoList() |
||||
{ |
||||
std::vector<UvcDeviceInfo> uvcDevList; |
||||
ComPtr<IMFAttributes> pAttributes = nullptr; |
||||
MFCreateAttributes(&pAttributes, 1); |
||||
pAttributes->SetGUID(MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE, MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE_VIDCAP_GUID); |
||||
// pAttributes->SetGUID(MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE_VIDCAP_CATEGORY, KSCATEGORY_SENSOR_CAMERA);
|
||||
IMFActivate** ppDevices; |
||||
uint32_t numDevices; |
||||
MFEnumDeviceSources(pAttributes.Get(), &ppDevices, &numDevices); |
||||
for (uint32_t i = 0; i < numDevices; ++i) |
||||
{ |
||||
ComPtr<IMFActivate> pDevice; |
||||
pDevice = ppDevices[i]; |
||||
|
||||
WCHAR* wCharStr = nullptr; |
||||
uint32_t length; |
||||
pDevice->GetAllocatedString(MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE_VIDCAP_SYMBOLIC_LINK, &wCharStr, &length); |
||||
auto symbolicLink = wideCharToUTF8(wCharStr); |
||||
CoTaskMemFree(wCharStr); |
||||
|
||||
pDevice->GetAllocatedString(MF_DEVSOURCE_ATTRIBUTE_FRIENDLY_NAME, &wCharStr, &length); |
||||
auto name = wideCharToUTF8(wCharStr); |
||||
CoTaskMemFree(wCharStr); |
||||
|
||||
uint16_t vid, pid, mi; |
||||
std::string uid, guid; |
||||
if (!parseUvcDeviceSymbolicLink(symbolicLink, vid, pid, mi, uid, guid)) |
||||
continue; |
||||
uvcDevList.emplace_back(UvcDeviceInfo({ symbolicLink, name, uid, vid, pid, mi })); |
||||
CV_LOG_INFO(NULL, "UVC device found: name=" << name << ", vid=" << vid << ", pid=" << pid << ", mi=" << mi << ", uid=" << uid << ", guid=" << guid); |
||||
} |
||||
return uvcDevList; |
||||
} |
||||
|
||||
Ptr<IStreamChannel> MFContext::createStreamChannel(const UvcDeviceInfo& devInfo) |
||||
{ |
||||
return makePtr<MSMFStreamChannel>(devInfo); |
||||
} |
||||
|
||||
MFContext::MFContext() |
||||
{ |
||||
CoInitialize(0); |
||||
CV_Assert(SUCCEEDED(MFStartup(MF_VERSION))); |
||||
} |
||||
|
||||
MSMFStreamChannel::MSMFStreamChannel(const UvcDeviceInfo& devInfo) : |
||||
IUvcStreamChannel(devInfo), |
||||
mfContext_(MFContext::getInstance()), |
||||
xuNodeId_(-1) |
||||
{ |
||||
HR_FAILED_RETURN(MFCreateAttributes(&deviceAttrs_, 2)); |
||||
HR_FAILED_RETURN(deviceAttrs_->SetGUID(MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE, MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE_VIDCAP_GUID)); |
||||
WCHAR* buffer = new wchar_t[devInfo_.id.length() + 1]; |
||||
MultiByteToWideChar(CP_UTF8, 0, devInfo_.id.c_str(), (int)devInfo_.id.length() + 1, buffer, (int)devInfo_.id.length() * sizeof(WCHAR)); |
||||
HR_FAILED_EXEC(deviceAttrs_->SetString(MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE_VIDCAP_SYMBOLIC_LINK, buffer), { |
||||
delete[] buffer; |
||||
return; |
||||
}) |
||||
delete[] buffer; |
||||
HR_FAILED_RETURN(MFCreateDeviceSource(deviceAttrs_.Get(), &deviceSource_)); |
||||
HR_FAILED_RETURN(deviceSource_->QueryInterface(__uuidof(IAMCameraControl), reinterpret_cast<void**>(&cameraControl_))); |
||||
HR_FAILED_RETURN(deviceSource_->QueryInterface(__uuidof(IAMVideoProcAmp), reinterpret_cast<void**>(&videoProcAmp_))); |
||||
|
||||
HR_FAILED_RETURN(MFCreateAttributes(&readerAttrs_, 3)); |
||||
HR_FAILED_RETURN(readerAttrs_->SetUINT32(MF_SOURCE_READER_DISCONNECT_MEDIASOURCE_ON_SHUTDOWN, false)); |
||||
HR_FAILED_RETURN(readerAttrs_->SetUINT32(MF_READWRITE_ENABLE_HARDWARE_TRANSFORMS, true)); |
||||
HR_FAILED_RETURN(readerAttrs_->SetUnknown(MF_SOURCE_READER_ASYNC_CALLBACK, static_cast<IUnknown*>(this))); |
||||
HR_FAILED_RETURN(MFCreateSourceReaderFromMediaSource(deviceSource_.Get(), readerAttrs_.Get(), &streamReader_)); |
||||
HR_FAILED_RETURN(streamReader_->SetStreamSelection(static_cast<DWORD>(MF_SOURCE_READER_ALL_STREAMS), true)); |
||||
|
||||
HR_FAILED_RETURN(deviceSource_->QueryInterface(__uuidof(IKsTopologyInfo), reinterpret_cast<void**>(&xuKsTopologyInfo_))); |
||||
DWORD nNodes = 0; |
||||
HR_FAILED_RETURN(xuKsTopologyInfo_->get_NumNodes(&nNodes)); |
||||
for (DWORD i = 0; i < nNodes; i++) |
||||
{ |
||||
GUID nodeType; |
||||
HR_FAILED_EXEC(xuKsTopologyInfo_->get_NodeType(i, &nodeType), { continue; }) |
||||
if (nodeType == KSNODETYPE_DEV_SPECIFIC) |
||||
{ |
||||
xuNodeId_ = i; |
||||
} |
||||
} |
||||
if (xuNodeId_ != -1) |
||||
{ |
||||
HR_FAILED_RETURN(xuKsTopologyInfo_->CreateNodeInstance(xuNodeId_, IID_IUnknown, reinterpret_cast<LPVOID*>(&xuNodeInstance_))); |
||||
HR_FAILED_RETURN(xuNodeInstance_->QueryInterface(__uuidof(IKsControl), reinterpret_cast<void**>(&xuKsControl_))); |
||||
} |
||||
|
||||
if (streamType_ == OBSENSOR_STREAM_DEPTH) |
||||
{ |
||||
initDepthFrameProcessor(); |
||||
} |
||||
} |
||||
|
||||
MSMFStreamChannel::~MSMFStreamChannel() |
||||
{ |
||||
stop(); |
||||
if (cameraControl_) |
||||
{ |
||||
cameraControl_.Release(); |
||||
} |
||||
if (videoProcAmp_) |
||||
{ |
||||
videoProcAmp_.Release(); |
||||
} |
||||
if (streamReader_) |
||||
{ |
||||
streamReader_.Release(); |
||||
} |
||||
if (readerAttrs_) |
||||
{ |
||||
readerAttrs_.Release(); |
||||
} |
||||
if (deviceAttrs_) |
||||
{ |
||||
deviceAttrs_.Release(); |
||||
} |
||||
if (deviceSource_) |
||||
{ |
||||
deviceSource_.Release(); |
||||
} |
||||
if (xuKsTopologyInfo_) |
||||
{ |
||||
xuKsTopologyInfo_.Release(); |
||||
} |
||||
if (xuNodeInstance_) |
||||
{ |
||||
xuNodeInstance_.Release(); |
||||
} |
||||
if (xuKsControl_) |
||||
{ |
||||
xuKsControl_.Release(); |
||||
} |
||||
} |
||||
|
||||
void MSMFStreamChannel::start(const StreamProfile& profile, FrameCallback frameCallback) |
||||
{ |
||||
ComPtr<IMFMediaType> mediaType = nullptr; |
||||
unsigned int width, height, fps; |
||||
FrameRate frameRateMin, frameRateMax; |
||||
bool quit = false; |
||||
|
||||
frameCallback_ = frameCallback; |
||||
currentProfile_ = profile; |
||||
currentStreamIndex_ = -1; |
||||
|
||||
for (uint8_t index = 0; index <= 5; index++) |
||||
{ |
||||
for (uint32_t k = 0;; k++) |
||||
{ |
||||
HR_FAILED_EXEC(streamReader_->GetNativeMediaType(index, k, &mediaType), { continue; }) |
||||
GUID subtype; |
||||
HR_FAILED_RETURN(mediaType->GetGUID(MF_MT_SUBTYPE, &subtype)); |
||||
HR_FAILED_RETURN(MFGetAttributeSize(mediaType.Get(), MF_MT_FRAME_SIZE, &width, &height)); |
||||
HR_FAILED_RETURN(MFGetAttributeRatio(mediaType.Get(), MF_MT_FRAME_RATE_RANGE_MIN, &frameRateMin.numerator, &frameRateMin.denominator)); |
||||
HR_FAILED_RETURN(MFGetAttributeRatio(mediaType.Get(), MF_MT_FRAME_RATE_RANGE_MAX, &frameRateMax.numerator, &frameRateMax.denominator)); |
||||
|
||||
if (static_cast<float>(frameRateMax.numerator) / frameRateMax.denominator < static_cast<float>(frameRateMin.numerator) / frameRateMin.denominator) |
||||
{ |
||||
std::swap(frameRateMax, frameRateMin); |
||||
} |
||||
|
||||
fps = frameRateMax.numerator / frameRateMax.denominator; |
||||
uint32_t device_fourcc = reinterpret_cast<const big_endian<uint32_t> &>(subtype.Data1); |
||||
if (width == profile.width && |
||||
height == profile.height && |
||||
fps == profile.fps && |
||||
frameFourccToFormat(device_fourcc) == profile.format) |
||||
{ |
||||
HR_FAILED_RETURN(streamReader_->SetCurrentMediaType(index, nullptr, mediaType.Get())); |
||||
HR_FAILED_RETURN(streamReader_->SetStreamSelection(index, true)); |
||||
streamReader_->ReadSample(index, 0, nullptr, nullptr, nullptr, nullptr); |
||||
|
||||
streamState_ = STREAM_STARTING; |
||||
currentStreamIndex_ = index; |
||||
quit = true; |
||||
|
||||
// wait for frame
|
||||
std::unique_lock<std::mutex> lock(streamStateMutex_); |
||||
auto success = streamStateCv_.wait_for(lock, std::chrono::milliseconds(3000), [&]() { |
||||
return streamState_ == STREAM_STARTED; |
||||
}); |
||||
if (!success) |
||||
{ |
||||
stop(); |
||||
} |
||||
break; |
||||
} |
||||
mediaType.Release(); |
||||
} |
||||
if (quit) |
||||
{ |
||||
break; |
||||
} |
||||
} |
||||
streamState_ = quit ? streamState_ : STREAM_STOPED; |
||||
} |
||||
|
||||
void MSMFStreamChannel::stop() |
||||
{ |
||||
if (streamState_ == STREAM_STARTING || streamState_ == STREAM_STARTED) |
||||
{ |
||||
streamState_ = STREAM_STOPPING; |
||||
streamReader_->SetStreamSelection(currentStreamIndex_, false); |
||||
streamReader_->Flush(currentStreamIndex_); |
||||
std::unique_lock<std::mutex> lk(streamStateMutex_); |
||||
streamStateCv_.wait_for(lk, std::chrono::milliseconds(1000), [&]() { |
||||
return streamState_ == STREAM_STOPED; |
||||
}); |
||||
} |
||||
} |
||||
|
||||
bool MSMFStreamChannel::setXu(uint8_t ctrl, const uint8_t* data, uint32_t len) |
||||
{ |
||||
if (xuSendBuf_.size() < XU_MAX_DATA_LENGTH) { |
||||
xuSendBuf_.resize(XU_MAX_DATA_LENGTH); |
||||
} |
||||
memcpy(xuSendBuf_.data(), data, len); |
||||
|
||||
KSP_NODE node; |
||||
memset(&node, 0, sizeof(KSP_NODE)); |
||||
node.Property.Set = { 0xA55751A1, 0xF3C5, 0x4A5E, {0x8D, 0x5A, 0x68, 0x54, 0xB8, 0xFA, 0x27, 0x16} }; |
||||
node.Property.Id = ctrl; |
||||
node.Property.Flags = KSPROPERTY_TYPE_SET | KSPROPERTY_TYPE_TOPOLOGY; |
||||
node.NodeId = xuNodeId_; |
||||
|
||||
ULONG bytes_received = 0; |
||||
HR_FAILED_EXEC(xuKsControl_->KsProperty(reinterpret_cast<PKSPROPERTY>(&node), sizeof(KSP_NODE), (void*)xuSendBuf_.data(), XU_MAX_DATA_LENGTH, &bytes_received), { |
||||
return false; |
||||
}); |
||||
return true; |
||||
} |
||||
|
||||
bool MSMFStreamChannel::getXu(uint8_t ctrl, uint8_t** data, uint32_t* len) |
||||
{ |
||||
if (xuRecvBuf_.size() < XU_MAX_DATA_LENGTH) { |
||||
xuRecvBuf_.resize(XU_MAX_DATA_LENGTH); |
||||
} |
||||
KSP_NODE node; |
||||
memset(&node, 0, sizeof(KSP_NODE)); |
||||
node.Property.Set = { 0xA55751A1, 0xF3C5, 0x4A5E, {0x8D, 0x5A, 0x68, 0x54, 0xB8, 0xFA, 0x27, 0x16} }; |
||||
node.Property.Id = ctrl; |
||||
node.Property.Flags = KSPROPERTY_TYPE_GET | KSPROPERTY_TYPE_TOPOLOGY; |
||||
node.NodeId = xuNodeId_; |
||||
|
||||
ULONG bytes_received = 0; |
||||
HR_FAILED_EXEC(xuKsControl_->KsProperty(reinterpret_cast<PKSPROPERTY>(&node), sizeof(node), xuRecvBuf_.data(), XU_MAX_DATA_LENGTH, &bytes_received), { |
||||
*len = 0; |
||||
data = nullptr; |
||||
return false; |
||||
}); |
||||
*data = xuRecvBuf_.data(); |
||||
*len = bytes_received; |
||||
return true; |
||||
} |
||||
|
||||
STDMETHODIMP MSMFStreamChannel::QueryInterface(REFIID iid, void** ppv) |
||||
{ |
||||
#pragma warning(push) |
||||
#pragma warning(disable : 4838) |
||||
static const QITAB qit[] = { |
||||
QITABENT(MSMFStreamChannel, IMFSourceReaderCallback), |
||||
{nullptr}, |
||||
}; |
||||
return QISearch(this, qit, iid, ppv); |
||||
#pragma warning(pop) |
||||
} |
||||
|
||||
STDMETHODIMP_(ULONG) |
||||
MSMFStreamChannel::AddRef() |
||||
{ |
||||
return InterlockedIncrement(&refCount_); |
||||
} |
||||
|
||||
STDMETHODIMP_(ULONG) |
||||
MSMFStreamChannel::Release() |
||||
{ |
||||
ULONG count = InterlockedDecrement(&refCount_); |
||||
if (count <= 0) |
||||
{ |
||||
delete this; |
||||
} |
||||
return count; |
||||
} |
||||
|
||||
STDMETHODIMP MSMFStreamChannel::OnReadSample(HRESULT hrStatus, DWORD dwStreamIndex, DWORD /*dwStreamFlags*/, LONGLONG /*timeStamp*/, IMFSample* sample) |
||||
{ |
||||
HR_FAILED_LOG(hrStatus); |
||||
|
||||
if (streamState_ == STREAM_STARTING) |
||||
{ |
||||
std::unique_lock<std::mutex> lock(streamStateMutex_); |
||||
streamState_ = STREAM_STARTED; |
||||
streamStateCv_.notify_all(); |
||||
} |
||||
|
||||
if (streamState_ != STREAM_STOPPING && streamState_ != STREAM_STOPED) |
||||
{ |
||||
HR_FAILED_LOG(streamReader_->ReadSample(dwStreamIndex, 0, nullptr, nullptr, nullptr, nullptr)); |
||||
if (sample) |
||||
{ |
||||
ComPtr<IMFMediaBuffer> buffer = nullptr; |
||||
DWORD max_length, current_length; |
||||
byte* byte_buffer = nullptr; |
||||
|
||||
HR_FAILED_EXEC(sample->GetBufferByIndex(0, &buffer), { return S_OK; }); |
||||
|
||||
buffer->Lock(&byte_buffer, &max_length, ¤t_length); |
||||
Frame fo = { currentProfile_.format, currentProfile_.width, currentProfile_.height, current_length, (uint8_t*)byte_buffer }; |
||||
if (depthFrameProcessor_) |
||||
{ |
||||
depthFrameProcessor_->process(&fo); |
||||
} |
||||
frameCallback_(&fo); |
||||
buffer->Unlock(); |
||||
} |
||||
} |
||||
return S_OK; |
||||
} |
||||
|
||||
STDMETHODIMP MSMFStreamChannel::OnEvent(DWORD /*sidx*/, IMFMediaEvent* /*event*/) |
||||
{ |
||||
return S_OK; |
||||
} |
||||
|
||||
STDMETHODIMP MSMFStreamChannel::OnFlush(DWORD) |
||||
{ |
||||
if (streamState_ == STREAM_STARTING) |
||||
{ |
||||
std::unique_lock<std::mutex> lock(streamStateMutex_); |
||||
streamState_ = STREAM_STOPED; |
||||
streamStateCv_.notify_all(); |
||||
} |
||||
return S_OK; |
||||
} |
||||
}} // namespace cv::obsensor::
|
||||
#endif // HAVE_OBSENSOR_MSMF
|
||||
@ -0,0 +1,180 @@ |
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html.
|
||||
|
||||
/*
|
||||
* Copyright(C) 2022 by ORBBEC Technology., Inc. |
||||
* Authors: |
||||
* Huang Zhenchang <yufeng@orbbec.com> |
||||
* |
||||
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||
* you may not use this file except in compliance with the License. |
||||
* You may obtain a copy of the License at |
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
* |
||||
* Unless required by applicable law or agreed to in writing, software |
||||
* distributed under the License is distributed on an "AS IS" BASIS, |
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||
* See the License for the specific language governing permissions and |
||||
* limitations under the License. |
||||
*/ |
||||
|
||||
#ifndef OPENCV_VIDEOIO_OBSENSOR_STREAM_CHANNEL_MSMF_HPP |
||||
#define OPENCV_VIDEOIO_OBSENSOR_STREAM_CHANNEL_MSMF_HPP |
||||
#ifdef HAVE_OBSENSOR_MSMF |
||||
|
||||
#include "obsensor_uvc_stream_channel.hpp" |
||||
|
||||
#include <windows.h> |
||||
#include <guiddef.h> |
||||
#include <mfapi.h> |
||||
#include <mfidl.h> |
||||
#include <mfplay.h> |
||||
#include <mfobjects.h> |
||||
#include <mfreadwrite.h> |
||||
#include <tchar.h> |
||||
#include <strsafe.h> |
||||
#include <codecvt> |
||||
#include <ks.h> |
||||
#include <comdef.h> |
||||
#include <mutex> |
||||
#include <vidcap.h> //IKsTopologyInfo |
||||
#include <ksproxy.h> //IKsControl |
||||
#include <ksmedia.h> |
||||
|
||||
namespace cv { |
||||
namespace obsensor { |
||||
template <class T> |
||||
class ComPtr |
||||
{ |
||||
public: |
||||
ComPtr() {} |
||||
ComPtr(T* lp) |
||||
{ |
||||
p = lp; |
||||
} |
||||
ComPtr(_In_ const ComPtr<T>& lp) |
||||
{ |
||||
p = lp.p; |
||||
} |
||||
virtual ~ComPtr() {} |
||||
|
||||
void swap(_In_ ComPtr<T>& lp) |
||||
{ |
||||
ComPtr<T> tmp(p); |
||||
p = lp.p; |
||||
lp.p = tmp.p; |
||||
tmp = NULL; |
||||
} |
||||
T** operator&() |
||||
{ |
||||
CV_Assert(p == NULL); |
||||
return p.operator&(); |
||||
} |
||||
T* operator->() const |
||||
{ |
||||
CV_Assert(p != NULL); |
||||
return p.operator->(); |
||||
} |
||||
operator bool() |
||||
{ |
||||
return p.operator!=(NULL); |
||||
} |
||||
|
||||
T* Get() const |
||||
{ |
||||
return p; |
||||
} |
||||
|
||||
void Release() |
||||
{ |
||||
if (p) |
||||
p.Release(); |
||||
} |
||||
|
||||
// query for U interface
|
||||
template <typename U> |
||||
HRESULT As(_Out_ ComPtr<U>& lp) const |
||||
{ |
||||
lp.Release(); |
||||
return p->QueryInterface(__uuidof(U), reinterpret_cast<void**>((T**)&lp)); |
||||
} |
||||
|
||||
private: |
||||
_COM_SMARTPTR_TYPEDEF(T, __uuidof(T)); |
||||
TPtr p; |
||||
}; |
||||
|
||||
class MFContext |
||||
{ |
||||
public: |
||||
~MFContext(void); |
||||
static MFContext& getInstance(); |
||||
|
||||
std::vector<UvcDeviceInfo> queryUvcDeviceInfoList(); |
||||
Ptr<IStreamChannel> createStreamChannel(const UvcDeviceInfo& devInfo); |
||||
|
||||
private: |
||||
MFContext(void); |
||||
}; |
||||
|
||||
struct FrameRate |
||||
{ |
||||
unsigned int denominator; |
||||
unsigned int numerator; |
||||
}; |
||||
|
||||
class MSMFStreamChannel : public IUvcStreamChannel, public IMFSourceReaderCallback |
||||
{ |
||||
public: |
||||
MSMFStreamChannel(const UvcDeviceInfo& devInfo); |
||||
virtual ~MSMFStreamChannel() noexcept; |
||||
|
||||
virtual void start(const StreamProfile& profile, FrameCallback frameCallback) override; |
||||
virtual void stop() override; |
||||
|
||||
private: |
||||
virtual bool setXu(uint8_t ctrl, const uint8_t* data, uint32_t len) override; |
||||
virtual bool getXu(uint8_t ctrl, uint8_t** data, uint32_t* len) override; |
||||
|
||||
private: |
||||
MFContext& mfContext_; |
||||
|
||||
ComPtr<IMFAttributes> deviceAttrs_ = nullptr; |
||||
ComPtr<IMFMediaSource> deviceSource_ = nullptr; |
||||
ComPtr<IMFAttributes> readerAttrs_ = nullptr; |
||||
ComPtr<IMFSourceReader> streamReader_ = nullptr; |
||||
ComPtr<IAMCameraControl> cameraControl_ = nullptr; |
||||
ComPtr<IAMVideoProcAmp> videoProcAmp_ = nullptr; |
||||
ComPtr<IKsTopologyInfo> xuKsTopologyInfo_ = nullptr; |
||||
ComPtr<IUnknown> xuNodeInstance_ = nullptr; |
||||
ComPtr<IKsControl> xuKsControl_ = nullptr; |
||||
int xuNodeId_; |
||||
|
||||
FrameCallback frameCallback_; |
||||
StreamProfile currentProfile_; |
||||
int8_t currentStreamIndex_; |
||||
|
||||
StreamState streamState_; |
||||
std::mutex streamStateMutex_; |
||||
std::condition_variable streamStateCv_; |
||||
|
||||
std::vector<uint8_t> xuRecvBuf_; |
||||
std::vector<uint8_t> xuSendBuf_; |
||||
|
||||
public: |
||||
STDMETHODIMP QueryInterface(REFIID iid, void** ppv) override; |
||||
STDMETHODIMP_(ULONG) |
||||
AddRef() override; |
||||
STDMETHODIMP_(ULONG) |
||||
Release() override; |
||||
STDMETHODIMP OnReadSample(HRESULT /*hrStatus*/, DWORD dwStreamIndex, DWORD /*dwStreamFlags*/, LONGLONG /*llTimestamp*/, IMFSample* sample) override; |
||||
STDMETHODIMP OnEvent(DWORD /*sidx*/, IMFMediaEvent* /*event*/) override; |
||||
STDMETHODIMP OnFlush(DWORD) override; |
||||
|
||||
private: |
||||
long refCount_ = 1; |
||||
}; |
||||
}} // namespace cv::obsensor::
|
||||
#endif // HAVE_OBSENSOR_MSMF
|
||||
#endif // OPENCV_VIDEOIO_OBSENSOR_STREAM_CHANNEL_MSMF_HPP
|
||||
@ -0,0 +1,379 @@ |
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html.
|
||||
|
||||
/*
|
||||
* Copyright(C) 2022 by ORBBEC Technology., Inc. |
||||
* Authors: |
||||
* Huang Zhenchang <yufeng@orbbec.com> |
||||
* |
||||
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||
* you may not use this file except in compliance with the License. |
||||
* You may obtain a copy of the License at |
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
* |
||||
* Unless required by applicable law or agreed to in writing, software |
||||
* distributed under the License is distributed on an "AS IS" BASIS, |
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||
* See the License for the specific language governing permissions and |
||||
* limitations under the License. |
||||
*/ |
||||
|
||||
#ifdef HAVE_OBSENSOR_V4L2 |
||||
#include "obsensor_stream_channel_v4l2.hpp" |
||||
|
||||
#include <errno.h> |
||||
#include <unistd.h> |
||||
#include <fcntl.h> |
||||
#include <sys/ioctl.h> |
||||
#include <sys/types.h> |
||||
#include <sys/mman.h> |
||||
#include <linux/videodev2.h> |
||||
#include <linux/uvcvideo.h> |
||||
#include <linux/usb/video.h> |
||||
#include <fstream> |
||||
#include <map> |
||||
#include <vector> |
||||
|
||||
#include "opencv2/core/utils/filesystem.hpp" |
||||
|
||||
namespace cv { |
||||
namespace obsensor { |
||||
|
||||
#define IOCTL_FAILED_RETURN(x) \ |
||||
if (x < 0) \
|
||||
{ \
|
||||
CV_LOG_WARNING(NULL, "ioctl error return: " << errno); \
|
||||
return; \
|
||||
} |
||||
|
||||
#define IOCTL_FAILED_LOG(x) \ |
||||
if (x < 0) \
|
||||
{ \
|
||||
CV_LOG_WARNING(NULL, "ioctl error return: " << errno); \
|
||||
} |
||||
|
||||
#define IOCTL_FAILED_CONTINUE(x) \ |
||||
if (x < 0) \
|
||||
{ \
|
||||
CV_LOG_WARNING(NULL, "ioctl error return: " << errno); \
|
||||
continue; \
|
||||
} |
||||
|
||||
#define IOCTL_FAILED_EXEC(x, statement) \ |
||||
if (x < 0) \
|
||||
{ \
|
||||
CV_LOG_WARNING(NULL, "ioctl error return: " << errno); \
|
||||
statement; \
|
||||
} |
||||
|
||||
int xioctl(int fd, int req, void* arg) |
||||
{ |
||||
int rst; |
||||
int retry = 5; |
||||
do |
||||
{ |
||||
rst = ioctl(fd, req, arg); |
||||
retry--; |
||||
} while (rst == -1 && (errno == EAGAIN || (errno == EBUSY && retry > 0))); |
||||
|
||||
if (rst < 0) |
||||
{ |
||||
CV_LOG_WARNING(NULL, "ioctl: fd=" << fd << ", req=" << req); |
||||
} |
||||
return rst; |
||||
} |
||||
|
||||
V4L2Context& V4L2Context::getInstance() |
||||
{ |
||||
static V4L2Context instance; |
||||
return instance; |
||||
} |
||||
|
||||
std::vector<UvcDeviceInfo> V4L2Context::queryUvcDeviceInfoList() |
||||
{ |
||||
std::vector<UvcDeviceInfo> uvcDevList; |
||||
std::map<std::string, UvcDeviceInfo> uvcDevMap; |
||||
const cv::String videosDir = "/sys/class/video4linux"; |
||||
cv::utils::Paths videos; |
||||
if (cv::utils::fs::isDirectory(videosDir)) |
||||
{ |
||||
cv::utils::fs::glob(videosDir, "*", videos, false, true); |
||||
for (const auto& video : videos) |
||||
{ |
||||
UvcDeviceInfo uvcDev; |
||||
cv::String videoName = video.substr(video.find_last_of("/") + 1); |
||||
char buf[PATH_MAX]; |
||||
if (realpath(video.c_str(), buf) == nullptr || cv::String(buf).find("virtual") != std::string::npos) |
||||
{ |
||||
continue; |
||||
} |
||||
cv::String videoRealPath = buf; |
||||
cv::String interfaceRealPath = videoRealPath.substr(0, videoRealPath.find_last_of("/")); |
||||
|
||||
std::string busNum, devNum, devPath; |
||||
while (videoRealPath.find_last_of("/") != std::string::npos) |
||||
{ |
||||
videoRealPath = videoRealPath.substr(0, videoRealPath.find_last_of("/")); |
||||
if (!(std::ifstream(videoRealPath + "/busnum") >> busNum)) |
||||
{ |
||||
continue; |
||||
} |
||||
if (!(std::ifstream(videoRealPath + "/devpath") >> devPath)) |
||||
{ |
||||
continue; |
||||
} |
||||
if (!(std::ifstream(videoRealPath + "/devnum") >> devNum)) |
||||
{ |
||||
continue; |
||||
} |
||||
uvcDev.uid = busNum + "-" + devPath + "-" + devNum; |
||||
break; |
||||
/* code */ |
||||
} |
||||
|
||||
uvcDev.id = cv::String("/dev/") + videoName; |
||||
v4l2_capability caps = {}; |
||||
int videoFd = open(uvcDev.id.c_str(), O_RDONLY); |
||||
IOCTL_FAILED_EXEC(xioctl(videoFd, VIDIOC_QUERYCAP, &caps), { |
||||
close(videoFd); |
||||
continue; |
||||
}); |
||||
close(videoFd); |
||||
|
||||
if (caps.capabilities & V4L2_CAP_VIDEO_CAPTURE) |
||||
{ |
||||
cv::String modalias; |
||||
if (!(std::ifstream(video + "/device/modalias") >> modalias) || |
||||
modalias.size() < 14 || |
||||
modalias.substr(0, 5) != "usb:v" || |
||||
modalias[9] != 'p') |
||||
{ |
||||
continue; |
||||
} |
||||
std::istringstream(modalias.substr(5, 4)) >> std::hex >> uvcDev.vid; |
||||
std::istringstream(modalias.substr(10, 4)) >> std::hex >> uvcDev.pid; |
||||
std::getline(std::ifstream(video + "/device/interface"), uvcDev.name); |
||||
std::ifstream(video + "/device/bInterfaceNumber") >> uvcDev.mi; |
||||
uvcDevMap.insert({ interfaceRealPath, uvcDev }); |
||||
} |
||||
} |
||||
} |
||||
for (const auto& item : uvcDevMap) |
||||
{ |
||||
const auto uvcDev = item.second; // alias
|
||||
CV_LOG_INFO(NULL, "UVC device found: name=" << uvcDev.name << ", vid=" << uvcDev.vid << ", pid=" << uvcDev.pid << ", mi=" << uvcDev.mi << ", uid=" << uvcDev.uid << ", id=" << uvcDev.id); |
||||
uvcDevList.push_back(uvcDev); |
||||
} |
||||
return uvcDevList; |
||||
} |
||||
|
||||
Ptr<IStreamChannel> V4L2Context::createStreamChannel(const UvcDeviceInfo& devInfo) |
||||
{ |
||||
return makePtr<V4L2StreamChannel>(devInfo); |
||||
} |
||||
|
||||
V4L2StreamChannel::V4L2StreamChannel(const UvcDeviceInfo &devInfo) : IUvcStreamChannel(devInfo), |
||||
devFd_(-1), |
||||
streamState_(STREAM_STOPED) |
||||
{ |
||||
|
||||
devFd_ = open(devInfo_.id.c_str(), O_RDWR | O_NONBLOCK, 0); |
||||
if (devFd_ < 0) |
||||
{ |
||||
CV_LOG_ERROR(NULL, "Open " << devInfo_.id << " failed ! errno=" << errno) |
||||
} |
||||
else if (streamType_ == OBSENSOR_STREAM_DEPTH) |
||||
{ |
||||
initDepthFrameProcessor(); |
||||
} |
||||
|
||||
} |
||||
|
||||
V4L2StreamChannel::~V4L2StreamChannel() noexcept |
||||
{ |
||||
stop(); |
||||
if (devFd_) |
||||
{ |
||||
close(devFd_); |
||||
devFd_ = -1; |
||||
} |
||||
} |
||||
|
||||
void V4L2StreamChannel::start(const StreamProfile& profile, FrameCallback frameCallback) |
||||
{ |
||||
if (streamState_ != STREAM_STOPED) |
||||
{ |
||||
CV_LOG_ERROR(NULL, devInfo_.id << ": repetitive operation!") |
||||
return; |
||||
} |
||||
frameCallback_ = frameCallback; |
||||
currentProfile_ = profile; |
||||
|
||||
struct v4l2_format fmt = {}; |
||||
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
||||
fmt.fmt.pix.width = profile.width; |
||||
fmt.fmt.pix.height = profile.height; |
||||
fmt.fmt.pix.pixelformat = frameFormatToFourcc(profile.format); |
||||
IOCTL_FAILED_RETURN(xioctl(devFd_, VIDIOC_S_FMT, &fmt)); |
||||
IOCTL_FAILED_RETURN(xioctl(devFd_, VIDIOC_G_FMT, &fmt)); |
||||
|
||||
struct v4l2_streamparm streamParm = {}; |
||||
streamParm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
||||
streamParm.parm.capture.timeperframe.numerator = 1; |
||||
streamParm.parm.capture.timeperframe.denominator = profile.fps; |
||||
IOCTL_FAILED_RETURN(xioctl(devFd_, VIDIOC_S_PARM, &streamParm)); |
||||
IOCTL_FAILED_RETURN(xioctl(devFd_, VIDIOC_G_PARM, &streamParm)); |
||||
|
||||
struct v4l2_requestbuffers req = {}; |
||||
req.count = MAX_FRAME_BUFFER_NUM; |
||||
req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
||||
req.memory = V4L2_MEMORY_MMAP; |
||||
IOCTL_FAILED_RETURN(xioctl(devFd_, VIDIOC_REQBUFS, &req)); |
||||
|
||||
for (uint32_t i = 0; i < req.count && i < MAX_FRAME_BUFFER_NUM; i++) |
||||
{ |
||||
struct v4l2_buffer buf = {}; |
||||
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
||||
buf.memory = V4L2_MEMORY_MMAP; |
||||
buf.index = i; // only one buffer
|
||||
IOCTL_FAILED_RETURN(xioctl(devFd_, VIDIOC_QUERYBUF, &buf)); |
||||
frameBuffList[i].ptr = (uint8_t*)mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, devFd_, buf.m.offset); |
||||
frameBuffList[i].length = buf.length; |
||||
} |
||||
|
||||
// stream on
|
||||
std::unique_lock<std::mutex> lk(streamStateMutex_); |
||||
streamState_ = STREAM_STARTING; |
||||
uint32_t type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
||||
IOCTL_FAILED_EXEC(xioctl(devFd_, VIDIOC_STREAMON, &type), { |
||||
streamState_ = STREAM_STOPED; |
||||
for (uint32_t i = 0; i < MAX_FRAME_BUFFER_NUM; i++) |
||||
{ |
||||
if (frameBuffList[i].ptr) |
||||
{ |
||||
munmap(frameBuffList[i].ptr, frameBuffList[i].length); |
||||
frameBuffList[i].ptr = nullptr; |
||||
frameBuffList[i].length = 0; |
||||
} |
||||
} |
||||
return; |
||||
}); |
||||
grabFrameThread_ = std::thread(&V4L2StreamChannel::grabFrame, this); |
||||
} |
||||
|
||||
void V4L2StreamChannel::grabFrame() |
||||
{ |
||||
fd_set fds; |
||||
FD_ZERO(&fds); |
||||
FD_SET(devFd_, &fds); |
||||
struct timeval tv = {}; |
||||
tv.tv_sec = 0; |
||||
tv.tv_usec = 100000; // 100ms
|
||||
|
||||
struct v4l2_buffer buf = {}; |
||||
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
||||
buf.memory = V4L2_MEMORY_MMAP; |
||||
// buf.index = 0;
|
||||
|
||||
IOCTL_FAILED_EXEC(xioctl(devFd_, VIDIOC_QBUF, &buf), { |
||||
std::unique_lock<std::mutex> lk(streamStateMutex_); |
||||
streamState_ = STREAM_STOPED; |
||||
streamStateCv_.notify_all(); |
||||
return; |
||||
}); |
||||
|
||||
while (streamState_ == STREAM_STARTING || streamState_ == STREAM_STARTED) |
||||
{ |
||||
IOCTL_FAILED_CONTINUE(select(devFd_ + 1, &fds, NULL, NULL, &tv)); |
||||
IOCTL_FAILED_CONTINUE(xioctl(devFd_, VIDIOC_DQBUF, &buf)); |
||||
if (streamState_ == STREAM_STARTING) |
||||
{ |
||||
std::unique_lock<std::mutex> lk(streamStateMutex_); |
||||
streamState_ = STREAM_STARTED; |
||||
streamStateCv_.notify_all(); |
||||
} |
||||
Frame fo = { currentProfile_.format, currentProfile_.width, currentProfile_.height, buf.length, frameBuffList[buf.index].ptr }; |
||||
if (depthFrameProcessor_) |
||||
{ |
||||
depthFrameProcessor_->process(&fo); |
||||
} |
||||
frameCallback_(&fo); |
||||
IOCTL_FAILED_CONTINUE(xioctl(devFd_, VIDIOC_QBUF, &buf)); |
||||
} |
||||
std::unique_lock<std::mutex> lk(streamStateMutex_); |
||||
streamState_ = STREAM_STOPED; |
||||
streamStateCv_.notify_all(); |
||||
} |
||||
|
||||
bool V4L2StreamChannel::setXu(uint8_t ctrl, const uint8_t* data, uint32_t len) |
||||
{ |
||||
if (xuSendBuf_.size() < XU_MAX_DATA_LENGTH) { |
||||
xuSendBuf_.resize(XU_MAX_DATA_LENGTH); |
||||
} |
||||
memcpy(xuSendBuf_.data(), data, len); |
||||
struct uvc_xu_control_query xu_ctrl_query = { |
||||
.unit = XU_UNIT_ID, |
||||
.selector = ctrl, |
||||
.query = UVC_SET_CUR, |
||||
.size = (__u16)(ctrl == 1 ? 512 : (ctrl == 2 ? 64 : 1024)), |
||||
.data = xuSendBuf_.data() |
||||
}; |
||||
if (devFd_ > 0) |
||||
{ |
||||
IOCTL_FAILED_EXEC(xioctl(devFd_, UVCIOC_CTRL_QUERY, &xu_ctrl_query), { return false; }); |
||||
} |
||||
return true; |
||||
} |
||||
|
||||
bool V4L2StreamChannel::getXu(uint8_t ctrl, uint8_t** data, uint32_t* len) |
||||
{ |
||||
if (xuRecvBuf_.size() < XU_MAX_DATA_LENGTH) { |
||||
xuRecvBuf_.resize(XU_MAX_DATA_LENGTH); |
||||
} |
||||
struct uvc_xu_control_query xu_ctrl_query = { |
||||
.unit = XU_UNIT_ID, |
||||
.selector = ctrl, |
||||
.query = UVC_GET_CUR, |
||||
.size = (__u16)(ctrl == 1 ? 512 : (ctrl == 2 ? 64 : 1024)), |
||||
.data = xuRecvBuf_.data() |
||||
}; |
||||
|
||||
IOCTL_FAILED_EXEC(xioctl(devFd_, UVCIOC_CTRL_QUERY, &xu_ctrl_query), { |
||||
*len = 0; |
||||
return false; |
||||
}); |
||||
|
||||
*len = xu_ctrl_query.size; |
||||
*data = xuRecvBuf_.data(); |
||||
return true; |
||||
} |
||||
|
||||
void V4L2StreamChannel::stop() |
||||
{ |
||||
if (streamState_ == STREAM_STARTING || streamState_ == STREAM_STARTED) |
||||
{ |
||||
streamState_ = STREAM_STOPPING; |
||||
std::unique_lock<std::mutex> lk(streamStateMutex_); |
||||
streamStateCv_.wait_for(lk, std::chrono::milliseconds(1000), [&](){ |
||||
return streamState_ == STREAM_STOPED; |
||||
}); |
||||
uint32_t type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
||||
IOCTL_FAILED_LOG(xioctl(devFd_, VIDIOC_STREAMOFF, &type)); |
||||
} |
||||
if (grabFrameThread_.joinable()) |
||||
{ |
||||
grabFrameThread_.join(); |
||||
} |
||||
for (uint32_t i = 0; i < MAX_FRAME_BUFFER_NUM; i++) |
||||
{ |
||||
if (frameBuffList[i].ptr) |
||||
{ |
||||
munmap(frameBuffList[i].ptr, frameBuffList[i].length); |
||||
frameBuffList[i].ptr = nullptr; |
||||
frameBuffList[i].length = 0; |
||||
} |
||||
} |
||||
} |
||||
}} // namespace cv::obsensor::
|
||||
#endif // HAVE_OBSENSOR_V4L2
|
||||
@ -0,0 +1,90 @@ |
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html.
|
||||
|
||||
/*
|
||||
* Copyright(C) 2022 by ORBBEC Technology., Inc. |
||||
* Authors: |
||||
* Huang Zhenchang <yufeng@orbbec.com> |
||||
* |
||||
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||
* you may not use this file except in compliance with the License. |
||||
* You may obtain a copy of the License at |
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
* |
||||
* Unless required by applicable law or agreed to in writing, software |
||||
* distributed under the License is distributed on an "AS IS" BASIS, |
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||
* See the License for the specific language governing permissions and |
||||
* limitations under the License. |
||||
*/ |
||||
|
||||
#ifndef OPENCV_VIDEOIO_OBSENSOR_STREAM_CHANNEL_V4L2_HPP |
||||
#define OPENCV_VIDEOIO_OBSENSOR_STREAM_CHANNEL_V4L2_HPP |
||||
#ifdef HAVE_OBSENSOR_V4L2 |
||||
|
||||
#include "obsensor_uvc_stream_channel.hpp" |
||||
|
||||
#include <mutex> |
||||
#include <condition_variable> |
||||
#include <thread> |
||||
|
||||
namespace cv { |
||||
namespace obsensor { |
||||
#define MAX_FRAME_BUFFER_NUM 4 |
||||
struct V4L2FrameBuffer |
||||
{ |
||||
uint32_t length = 0; |
||||
uint8_t* ptr = nullptr; |
||||
}; |
||||
|
||||
int xioctl(int fd, int req, void* arg); |
||||
|
||||
class V4L2Context |
||||
{ |
||||
public: |
||||
~V4L2Context() {} |
||||
static V4L2Context& getInstance(); |
||||
|
||||
std::vector<UvcDeviceInfo> queryUvcDeviceInfoList(); |
||||
Ptr<IStreamChannel> createStreamChannel(const UvcDeviceInfo& devInfo); |
||||
|
||||
private: |
||||
V4L2Context() noexcept {} |
||||
}; |
||||
|
||||
class V4L2StreamChannel : public IUvcStreamChannel |
||||
{ |
||||
public: |
||||
V4L2StreamChannel(const UvcDeviceInfo& devInfo); |
||||
virtual ~V4L2StreamChannel() noexcept; |
||||
|
||||
virtual void start(const StreamProfile& profile, FrameCallback frameCallback) override; |
||||
virtual void stop() override; |
||||
|
||||
private: |
||||
void grabFrame(); |
||||
|
||||
virtual bool setXu(uint8_t ctrl, const uint8_t* data, uint32_t len) override; |
||||
virtual bool getXu(uint8_t ctrl, uint8_t** data, uint32_t* len) override; |
||||
|
||||
private: |
||||
int devFd_; |
||||
|
||||
V4L2FrameBuffer frameBuffList[MAX_FRAME_BUFFER_NUM]; |
||||
|
||||
StreamState streamState_; |
||||
std::mutex streamStateMutex_; |
||||
std::condition_variable streamStateCv_; |
||||
|
||||
std::thread grabFrameThread_; |
||||
|
||||
FrameCallback frameCallback_; |
||||
StreamProfile currentProfile_; |
||||
|
||||
std::vector<uint8_t> xuRecvBuf_; |
||||
std::vector<uint8_t> xuSendBuf_; |
||||
}; |
||||
}} // namespace cv::obsensor::
|
||||
#endif // HAVE_OBSENSOR_V4L2
|
||||
#endif // OPENCV_VIDEOIO_OBSENSOR_STREAM_CHANNEL_V4L2_HPP
|
||||
@ -0,0 +1,265 @@ |
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html.
|
||||
|
||||
/*
|
||||
* Copyright(C) 2022 by ORBBEC Technology., Inc. |
||||
* Authors: |
||||
* Huang Zhenchang <yufeng@orbbec.com> |
||||
* |
||||
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||
* you may not use this file except in compliance with the License. |
||||
* You may obtain a copy of the License at |
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
* |
||||
* Unless required by applicable law or agreed to in writing, software |
||||
* distributed under the License is distributed on an "AS IS" BASIS, |
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||
* See the License for the specific language governing permissions and |
||||
* limitations under the License. |
||||
*/ |
||||
|
||||
#if defined(HAVE_OBSENSOR_V4L2) || defined(HAVE_OBSENSOR_MSMF) |
||||
|
||||
#include <map> |
||||
#include <vector> |
||||
#include <string> |
||||
#include <algorithm> |
||||
#include <iterator> |
||||
|
||||
#if defined(HAVE_OBSENSOR_V4L2) |
||||
#include "obsensor_stream_channel_v4l2.hpp" |
||||
#elif defined(HAVE_OBSENSOR_MSMF) |
||||
#include "obsensor_stream_channel_msmf.hpp" |
||||
#endif // HAVE_OBSENSOR_V4L2
|
||||
|
||||
namespace cv { |
||||
namespace obsensor { |
||||
const uint8_t OB_EXT_CMD0[16] = { 0x47, 0x4d, 0x04, 0x00, 0x02, 0x00, 0x52, 0x00, 0x5B, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00 }; |
||||
const uint8_t OB_EXT_CMD1[16] = { 0x47, 0x4d, 0x04, 0x00, 0x02, 0x00, 0x54, 0x00, 0x3f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; |
||||
const uint8_t OB_EXT_CMD2[16] = { 0x47, 0x4d, 0x04, 0x00, 0x02, 0x00, 0x56, 0x00, 0x0d, 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_CMD5[16] = { 0x47, 0x4d, 0x02, 0x00, 0x03, 0x00, 0x62, 0x00, 0xe9, 0x03, 0x00, 0x00 }; |
||||
|
||||
#if defined(HAVE_OBSENSOR_V4L2) |
||||
#define fourCc2Int(a, b, c, d) \ |
||||
((uint32_t)(a) | ((uint32_t)(b) << 8) | ((uint32_t)(c) << 16) | ((uint32_t)(d) << 24)) |
||||
#elif defined(HAVE_OBSENSOR_MSMF) |
||||
#define fourCc2Int(a, b, c, d) \ |
||||
(((uint32_t)(a) <<24) | ((uint32_t)(b) << 16) | ((uint32_t)(c) << 8) | (uint32_t)(d)) |
||||
#endif // HAVE_OBSENSOR_V4L2
|
||||
|
||||
const std::map<uint32_t, FrameFormat> fourccToOBFormat = { |
||||
{fourCc2Int('Y', 'U', 'Y', '2'), FRAME_FORMAT_YUYV}, |
||||
{fourCc2Int('M', 'J', 'P', 'G'), FRAME_FORMAT_MJPG}, |
||||
{fourCc2Int('Y', '1', '6', ' '), FRAME_FORMAT_Y16}, |
||||
}; |
||||
|
||||
StreamType parseUvcDeviceNameToStreamType(const std::string& devName) |
||||
{ |
||||
std::string uvcDevName = devName; |
||||
for (size_t i = 0; i < uvcDevName.length(); i++) |
||||
{ |
||||
uvcDevName[i] = (char)tolower(uvcDevName[i]);
|
||||
} |
||||
if (uvcDevName.find(" depth") != std::string::npos) |
||||
{ |
||||
return OBSENSOR_STREAM_DEPTH; |
||||
} |
||||
else if (uvcDevName.find(" ir") != std::string::npos) |
||||
{ |
||||
return OBSENSOR_STREAM_IR; |
||||
} |
||||
|
||||
return OBSENSOR_STREAM_COLOR; // else
|
||||
} |
||||
|
||||
FrameFormat frameFourccToFormat(uint32_t fourcc) |
||||
{ |
||||
for (const auto& item : fourccToOBFormat) |
||||
{ |
||||
if (item.first == fourcc) |
||||
{ |
||||
return item.second; |
||||
} |
||||
} |
||||
return FRAME_FORMAT_UNKNOWN; |
||||
} |
||||
|
||||
uint32_t frameFormatToFourcc(FrameFormat fmt) |
||||
{ |
||||
for (const auto& item : fourccToOBFormat) |
||||
{ |
||||
if (item.second == fmt) |
||||
{ |
||||
return item.first; |
||||
} |
||||
} |
||||
return 0; |
||||
} |
||||
|
||||
std::vector<Ptr<IStreamChannel>> getStreamChannelGroup(uint32_t groupIdx) |
||||
{ |
||||
std::vector<Ptr<IStreamChannel>> streamChannelGroup; |
||||
|
||||
#if defined(HAVE_OBSENSOR_V4L2) |
||||
auto& ctx = V4L2Context::getInstance(); |
||||
#elif defined(HAVE_OBSENSOR_MSMF) |
||||
auto& ctx = MFContext::getInstance(); |
||||
#endif // HAVE_OBSENSOR_V4L2
|
||||
|
||||
auto uvcDevInfoList = ctx.queryUvcDeviceInfoList(); |
||||
|
||||
std::map<std::string, std::vector<UvcDeviceInfo>> uvcDevInfoGroupMap; |
||||
|
||||
auto devInfoIter = uvcDevInfoList.begin(); |
||||
while (devInfoIter != uvcDevInfoList.end()) |
||||
{ |
||||
if (devInfoIter->vid != OBSENSOR_CAM_VID) |
||||
{ |
||||
devInfoIter = uvcDevInfoList.erase(devInfoIter); // drop it
|
||||
continue; |
||||
} |
||||
devInfoIter++; |
||||
} |
||||
|
||||
if (!uvcDevInfoList.empty() && uvcDevInfoList.size() <= 3) |
||||
{ |
||||
uvcDevInfoGroupMap.insert({ "default", uvcDevInfoList }); |
||||
} |
||||
else { |
||||
for (auto& devInfo : uvcDevInfoList) |
||||
{ |
||||
// group by uid
|
||||
uvcDevInfoGroupMap[devInfo.uid].push_back(devInfo); // todo: group by sn
|
||||
} |
||||
} |
||||
|
||||
if (uvcDevInfoGroupMap.size() > groupIdx) |
||||
{ |
||||
auto uvcDevInfoGroupIter = uvcDevInfoGroupMap.begin(); |
||||
std::advance(uvcDevInfoGroupIter, groupIdx); |
||||
for (const auto& devInfo : uvcDevInfoGroupIter->second) |
||||
{ |
||||
streamChannelGroup.emplace_back(ctx.createStreamChannel(devInfo)); |
||||
} |
||||
} |
||||
else |
||||
{ |
||||
CV_LOG_ERROR(NULL, "Camera index out of range"); |
||||
} |
||||
return streamChannelGroup; |
||||
} |
||||
|
||||
DepthFrameProcessor::DepthFrameProcessor(const OBExtensionParam& param) : param_(param) |
||||
{ |
||||
double tempValue = 0; |
||||
double rstValue = 0; |
||||
lookUpTable_ = new uint16_t[4096]; |
||||
memset(lookUpTable_, 0, 4096 * 2); |
||||
for (uint16_t oriValue = 0; oriValue < 4096; oriValue++) |
||||
{ |
||||
if (oriValue == 0) |
||||
{ |
||||
continue; |
||||
} |
||||
tempValue = 200.375 - (double)oriValue / 8; |
||||
rstValue = (double)param_.pd / (1 + tempValue * param_.ps / param_.bl) * 10; |
||||
if ((rstValue >= 40) && (rstValue <= 10000) && rstValue < 65536) |
||||
{ |
||||
lookUpTable_[oriValue] = (uint16_t)rstValue; |
||||
} |
||||
} |
||||
} |
||||
|
||||
DepthFrameProcessor::~DepthFrameProcessor() |
||||
{ |
||||
delete[] lookUpTable_; |
||||
} |
||||
|
||||
void DepthFrameProcessor::process(Frame* frame) |
||||
{ |
||||
uint16_t* data = (uint16_t*)frame->data; |
||||
for (uint32_t i = 0; i < frame->dataSize / 2; i++) |
||||
{ |
||||
data[i] = lookUpTable_[data[i] & 0x0fff]; |
||||
} |
||||
} |
||||
|
||||
IUvcStreamChannel::IUvcStreamChannel(const UvcDeviceInfo& devInfo) : |
||||
devInfo_(devInfo), |
||||
streamType_(parseUvcDeviceNameToStreamType(devInfo_.name)) |
||||
{ |
||||
|
||||
} |
||||
|
||||
StreamType IUvcStreamChannel::streamType() const { |
||||
return streamType_; |
||||
} |
||||
|
||||
bool IUvcStreamChannel::setProperty(int propId, const uint8_t* /*data*/, uint32_t /*dataSize*/) |
||||
{ |
||||
uint8_t* rcvData; |
||||
uint32_t rcvLen; |
||||
bool rst = true; |
||||
switch (propId) |
||||
{ |
||||
case DEPTH_TO_COLOR_ALIGN: |
||||
// todo: value filling
|
||||
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; |
||||
default: |
||||
rst = false; |
||||
break; |
||||
} |
||||
return rst; |
||||
} |
||||
|
||||
bool IUvcStreamChannel::getProperty(int propId, uint8_t* recvData, uint32_t* recvDataSize) |
||||
{ |
||||
bool rst = true; |
||||
uint8_t* rcvData; |
||||
uint32_t rcvLen; |
||||
switch (propId) |
||||
{ |
||||
case CAMERA_PARAM: |
||||
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; |
||||
default: |
||||
rst = false; |
||||
break; |
||||
} |
||||
|
||||
return rst; |
||||
} |
||||
|
||||
bool IUvcStreamChannel::initDepthFrameProcessor() |
||||
{ |
||||
if (streamType_ == OBSENSOR_STREAM_DEPTH && setXu(2, OB_EXT_CMD4, sizeof(OB_EXT_CMD4))) |
||||
{ |
||||
uint8_t* rcvData; |
||||
uint32_t rcvLen; |
||||
if (getXu(1, &rcvData, &rcvLen) && OB_EXT_CMD4[6] == rcvData[6] && rcvData[8] == 0 && rcvData[9] == 0) |
||||
{ |
||||
depthFrameProcessor_ = makePtr<DepthFrameProcessor>(*(OBExtensionParam*)(rcvData + 10)); |
||||
return true; |
||||
} |
||||
} |
||||
return false; |
||||
} |
||||
}} // namespace cv::obsensor::
|
||||
#endif // HAVE_OBSENSOR_V4L2 || HAVE_OBSENSOR_MSMF
|
||||
@ -0,0 +1,96 @@ |
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html.
|
||||
|
||||
/*
|
||||
* Copyright(C) 2022 by ORBBEC Technology., Inc. |
||||
* Authors: |
||||
* Huang Zhenchang <yufeng@orbbec.com> |
||||
* |
||||
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||
* you may not use this file except in compliance with the License. |
||||
* You may obtain a copy of the License at |
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
* |
||||
* Unless required by applicable law or agreed to in writing, software |
||||
* distributed under the License is distributed on an "AS IS" BASIS, |
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||
* See the License for the specific language governing permissions and |
||||
* limitations under the License. |
||||
*/ |
||||
|
||||
#ifndef OPENCV_VIDEOIO_OBSENSOR_UVC_STREAM_CHANNEL_HPP |
||||
#define OPENCV_VIDEOIO_OBSENSOR_UVC_STREAM_CHANNEL_HPP |
||||
#include "obsensor_stream_channel_interface.hpp" |
||||
|
||||
#ifdef HAVE_OBSENSOR |
||||
namespace cv { |
||||
namespace obsensor { |
||||
|
||||
#define OBSENSOR_CAM_VID 0x2bc5 // usb vid
|
||||
#define XU_MAX_DATA_LENGTH 1024 |
||||
#define XU_UNIT_ID 4 |
||||
|
||||
struct UvcDeviceInfo |
||||
{ |
||||
std::string id = ""; // uvc sub-device id
|
||||
std::string name = ""; |
||||
std::string uid = ""; // parent usb device id
|
||||
uint16_t vid = 0; |
||||
uint16_t pid = 0; |
||||
uint16_t mi = 0; // uvc interface index
|
||||
}; |
||||
|
||||
enum StreamState |
||||
{ |
||||
STREAM_STOPED = 0, // stoped or ready
|
||||
STREAM_STARTING = 1, |
||||
STREAM_STARTED = 2, |
||||
STREAM_STOPPING = 3, |
||||
}; |
||||
|
||||
StreamType parseUvcDeviceNameToStreamType(const std::string& devName); |
||||
FrameFormat frameFourccToFormat(uint32_t fourcc); |
||||
uint32_t frameFormatToFourcc(FrameFormat); |
||||
|
||||
struct OBExtensionParam { |
||||
float bl; |
||||
float bl2; |
||||
float pd; |
||||
float ps; |
||||
}; |
||||
|
||||
class DepthFrameProcessor { |
||||
public: |
||||
DepthFrameProcessor(const OBExtensionParam& parma); |
||||
~DepthFrameProcessor() noexcept; |
||||
void process(Frame* frame); |
||||
|
||||
private: |
||||
const OBExtensionParam param_; |
||||
uint16_t* lookUpTable_; |
||||
}; |
||||
|
||||
class IUvcStreamChannel : public IStreamChannel { |
||||
public: |
||||
IUvcStreamChannel(const UvcDeviceInfo& devInfo); |
||||
virtual ~IUvcStreamChannel() noexcept {} |
||||
|
||||
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 StreamType streamType() const override; |
||||
|
||||
protected: |
||||
virtual bool setXu(uint8_t ctrl, const uint8_t* data, uint32_t len) = 0; |
||||
virtual bool getXu(uint8_t ctrl, uint8_t** data, uint32_t* len) = 0; |
||||
|
||||
bool initDepthFrameProcessor(); |
||||
|
||||
protected: |
||||
const UvcDeviceInfo devInfo_; |
||||
StreamType streamType_; |
||||
Ptr<DepthFrameProcessor> depthFrameProcessor_; |
||||
}; |
||||
}} // namespace cv::obsensor::
|
||||
#endif // HAVE_OBSENSOR
|
||||
#endif // OPENCV_VIDEOIO_OBSENSOR_UVC_STREAM_CHANNEL_HPP
|
||||
@ -0,0 +1,150 @@ |
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html.
|
||||
|
||||
/*
|
||||
* Copyright(C) 2022 by ORBBEC Technology., Inc. |
||||
* Authors: |
||||
* Huang Zhenchang <yufeng@orbbec.com> |
||||
* |
||||
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||
* you may not use this file except in compliance with the License. |
||||
* You may obtain a copy of the License at |
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
* |
||||
* Unless required by applicable law or agreed to in writing, software |
||||
* distributed under the License is distributed on an "AS IS" BASIS, |
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||
* See the License for the specific language governing permissions and |
||||
* limitations under the License. |
||||
*/ |
||||
|
||||
#include "precomp.hpp" |
||||
|
||||
#include "cap_obsensor_capture.hpp" |
||||
#include "cap_obsensor/obsensor_stream_channel_interface.hpp" |
||||
#ifdef HAVE_OBSENSOR |
||||
namespace cv { |
||||
Ptr<IVideoCapture> create_obsensor_capture(int index) |
||||
{ |
||||
return makePtr<VideoCapture_obsensor>(index); |
||||
} |
||||
|
||||
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}; |
||||
|
||||
streamChannelGroup_ = obsensor::getStreamChannelGroup(index); |
||||
if (!streamChannelGroup_.empty()) |
||||
{ |
||||
for (auto& channel : streamChannelGroup_) |
||||
{ |
||||
auto streamType = channel->streamType(); |
||||
switch (streamType) |
||||
{ |
||||
case obsensor::OBSENSOR_STREAM_COLOR: |
||||
channel->start(colorProfile, [&](obsensor::Frame* frame) { |
||||
std::unique_lock<std::mutex> lk(frameMutex_); |
||||
colorFrame_ = Mat(1, frame->dataSize, CV_8UC1, frame->data).clone(); |
||||
}); |
||||
break; |
||||
case obsensor::OBSENSOR_STREAM_DEPTH: |
||||
{ |
||||
uint8_t data = 1; |
||||
channel->setProperty(obsensor::DEPTH_TO_COLOR_ALIGN, &data, 1); |
||||
channel->start(depthProfile, [&](obsensor::Frame* frame) { |
||||
std::unique_lock<std::mutex> lk(frameMutex_); |
||||
depthFrame_ = Mat(frame->height, frame->width, CV_16UC1, frame->data, frame->width * 2).clone(); |
||||
}); |
||||
|
||||
uint32_t len; |
||||
memset(&camParam_, 0, sizeof(camParam_)); |
||||
channel->getProperty(obsensor::CAMERA_PARAM, (uint8_t*)&camParam_, &len); |
||||
camParamScale_ = (int)(camParam_.p1[2] * 2 / 640 + 0.5); |
||||
} |
||||
break; |
||||
default: |
||||
break; |
||||
} |
||||
} |
||||
isOpened_ = true; |
||||
} |
||||
} |
||||
|
||||
bool VideoCapture_obsensor::grabFrame() |
||||
{ |
||||
std::unique_lock<std::mutex> lk(frameMutex_); |
||||
|
||||
grabbedDepthFrame_ = depthFrame_; |
||||
grabbedColorFrame_ = colorFrame_; |
||||
|
||||
depthFrame_.release(); |
||||
colorFrame_.release(); |
||||
|
||||
return !grabbedDepthFrame_.empty() || !grabbedColorFrame_.empty(); |
||||
} |
||||
|
||||
bool VideoCapture_obsensor::retrieveFrame(int outputType, OutputArray frame) |
||||
{ |
||||
std::unique_lock<std::mutex> lk(frameMutex_); |
||||
switch (outputType) |
||||
{ |
||||
case CAP_OBSENSOR_DEPTH_MAP: |
||||
if (!grabbedDepthFrame_.empty()) |
||||
{ |
||||
grabbedDepthFrame_.copyTo(frame); |
||||
grabbedDepthFrame_.release(); |
||||
return true; |
||||
} |
||||
break; |
||||
case CAP_OBSENSOR_BGR_IMAGE: |
||||
if (!grabbedColorFrame_.empty()) |
||||
{ |
||||
auto mat = imdecode(grabbedColorFrame_, IMREAD_COLOR); |
||||
grabbedColorFrame_.release(); |
||||
|
||||
if (!mat.empty()) |
||||
{ |
||||
mat.copyTo(frame); |
||||
return true; |
||||
} |
||||
} |
||||
break; |
||||
default: |
||||
break; |
||||
} |
||||
|
||||
return false; |
||||
} |
||||
|
||||
double VideoCapture_obsensor::getProperty(int propIdx) const { |
||||
double rst = 0.0; |
||||
propIdx = propIdx & (~CAP_OBSENSOR_GENERATORS_MASK); |
||||
// int gen = propIdx & CAP_OBSENSOR_GENERATORS_MASK;
|
||||
switch (propIdx) |
||||
{ |
||||
case CAP_PROP_OBSENSOR_INTRINSIC_FX: |
||||
rst = camParam_.p1[0] / camParamScale_; |
||||
break; |
||||
case CAP_PROP_OBSENSOR_INTRINSIC_FY: |
||||
rst = camParam_.p1[1] / camParamScale_; |
||||
break; |
||||
case CAP_PROP_OBSENSOR_INTRINSIC_CX: |
||||
rst = camParam_.p1[2] / camParamScale_; |
||||
break; |
||||
case CAP_PROP_OBSENSOR_INTRINSIC_CY: |
||||
rst = camParam_.p1[3] / camParamScale_; |
||||
break; |
||||
} |
||||
return rst; |
||||
} |
||||
|
||||
bool VideoCapture_obsensor::setProperty(int propIdx, double /*propVal*/) |
||||
{ |
||||
CV_LOG_WARNING(NULL, "Unsupported or read only property, id=" << propIdx); |
||||
return false; |
||||
} |
||||
|
||||
} // namespace cv::
|
||||
#endif // HAVE_OBSENSOR
|
||||
@ -0,0 +1,66 @@ |
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html.
|
||||
|
||||
/*
|
||||
* Copyright(C) 2022 by ORBBEC Technology., Inc. |
||||
* Authors: |
||||
* Huang Zhenchang <yufeng@orbbec.com> |
||||
* |
||||
* Licensed under the Apache License, Version 2.0 (the "License"); |
||||
* you may not use this file except in compliance with the License. |
||||
* You may obtain a copy of the License at |
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
* |
||||
* Unless required by applicable law or agreed to in writing, software |
||||
* distributed under the License is distributed on an "AS IS" BASIS, |
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
||||
* See the License for the specific language governing permissions and |
||||
* limitations under the License. |
||||
*/ |
||||
|
||||
#ifndef OPENCV_VIDEOIO_CAP_OBSENSOR_CAPTURE_HPP |
||||
#define OPENCV_VIDEOIO_CAP_OBSENSOR_CAPTURE_HPP |
||||
|
||||
#include <map> |
||||
#include <mutex> |
||||
|
||||
#include "cap_obsensor/obsensor_stream_channel_interface.hpp" |
||||
|
||||
#ifdef HAVE_OBSENSOR |
||||
namespace cv { |
||||
class VideoCapture_obsensor : public IVideoCapture |
||||
{ |
||||
public: |
||||
VideoCapture_obsensor(int index); |
||||
virtual ~VideoCapture_obsensor() {} |
||||
|
||||
virtual double getProperty(int propIdx) const CV_OVERRIDE; |
||||
virtual bool setProperty(int propIdx, double propVal) CV_OVERRIDE; |
||||
virtual bool grabFrame() CV_OVERRIDE; |
||||
virtual bool retrieveFrame(int outputType, OutputArray frame) CV_OVERRIDE; |
||||
virtual int getCaptureDomain() CV_OVERRIDE { |
||||
return CAP_OBSENSOR; |
||||
} |
||||
virtual bool isOpened() const CV_OVERRIDE { |
||||
return isOpened_; |
||||
} |
||||
|
||||
private: |
||||
bool isOpened_; |
||||
std::vector<Ptr<obsensor::IStreamChannel>> streamChannelGroup_; |
||||
|
||||
std::mutex frameMutex_; |
||||
|
||||
Mat depthFrame_; |
||||
Mat colorFrame_; |
||||
|
||||
Mat grabbedDepthFrame_; |
||||
Mat grabbedColorFrame_; |
||||
|
||||
obsensor::CameraParam camParam_; |
||||
int camParamScale_; |
||||
}; |
||||
} // namespace cv::
|
||||
#endif // HAVE_OBSENSOR
|
||||
#endif // OPENCV_VIDEOIO_CAP_OBSENSOR_CAPTURE_HPP
|
||||
@ -0,0 +1,74 @@ |
||||
#include <opencv2/videoio.hpp> |
||||
#include <opencv2/highgui.hpp> |
||||
#include <opencv2/imgproc.hpp> |
||||
#include <iostream> |
||||
|
||||
using namespace cv; |
||||
int main() |
||||
{ |
||||
VideoCapture obsensorCapture(0, CAP_OBSENSOR); |
||||
if(!obsensorCapture.isOpened()){ |
||||
std::cerr << "Failed to open obsensor capture! Index out of range or no response from device"; |
||||
return -1; |
||||
} |
||||
|
||||
double fx = obsensorCapture.get(CAP_PROP_OBSENSOR_INTRINSIC_FX); |
||||
double fy = obsensorCapture.get(CAP_PROP_OBSENSOR_INTRINSIC_FY); |
||||
double cx = obsensorCapture.get(CAP_PROP_OBSENSOR_INTRINSIC_CX); |
||||
double cy = obsensorCapture.get(CAP_PROP_OBSENSOR_INTRINSIC_CY); |
||||
std::cout << "obsensor camera intrinsic params: fx=" << fx << ", fy=" << fy << ", cx=" << cx << ", cy=" << cy << std::endl; |
||||
|
||||
Mat image; |
||||
Mat depthMap; |
||||
Mat adjDepthMap; |
||||
while (true) |
||||
{ |
||||
// Grab depth map like this:
|
||||
// obsensorCapture >> depthMap;
|
||||
|
||||
// Another way to grab depth map (and bgr image).
|
||||
if (obsensorCapture.grab()) |
||||
{ |
||||
if (obsensorCapture.retrieve(image, CAP_OBSENSOR_BGR_IMAGE)) |
||||
{ |
||||
imshow("RGB", image); |
||||
} |
||||
|
||||
if (obsensorCapture.retrieve(depthMap, CAP_OBSENSOR_DEPTH_MAP)) |
||||
{ |
||||
normalize(depthMap, adjDepthMap, 0, 255, NORM_MINMAX, CV_8UC1); |
||||
applyColorMap(adjDepthMap, adjDepthMap, COLORMAP_JET); |
||||
imshow("DEPTH", adjDepthMap); |
||||
} |
||||
|
||||
// depth map overlay on bgr image
|
||||
static const float alpha = 0.6f; |
||||
if (!image.empty() && !depthMap.empty()) |
||||
{ |
||||
normalize(depthMap, adjDepthMap, 0, 255, NORM_MINMAX, CV_8UC1); |
||||
cv::resize(adjDepthMap, adjDepthMap, cv::Size(image.cols, image.rows)); |
||||
for (int i = 0; i < image.rows; i++) |
||||
{ |
||||
for (int j = 0; j < image.cols; j++) |
||||
{ |
||||
cv::Vec3b& outRgb = image.at<cv::Vec3b>(i, j); |
||||
uint8_t depthValue = 255 - adjDepthMap.at<uint8_t>(i, j); |
||||
if (depthValue != 0 && depthValue != 255) |
||||
{ |
||||
outRgb[0] = (uint8_t)(outRgb[0] * (1.0f - alpha) + depthValue * alpha); |
||||
outRgb[1] = (uint8_t)(outRgb[1] * (1.0f - alpha) + depthValue * alpha); |
||||
outRgb[2] = (uint8_t)(outRgb[2] * (1.0f - alpha) + depthValue * alpha); |
||||
} |
||||
} |
||||
} |
||||
imshow("DepthToColor", image); |
||||
} |
||||
image.release(); |
||||
depthMap.release(); |
||||
} |
||||
|
||||
if (pollKey() >= 0) |
||||
break; |
||||
} |
||||
return 0; |
||||
} |
||||
Loading…
Reference in new issue