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 error

CMakeLists.txt

@@ -459,6 +459,9 @@ OCV_OPTION(WITH_ONNX "Include Microsoft ONNX Runtime support" OFF
 OCV_OPTION(WITH_TIMVX "Include Tim-VX support" OFF
   VISIBLE_IF TRUE
   VERIFY HAVE_TIMVX)
+OCV_OPTION(WITH_OBSENSOR "Include obsensor support (Orbbec RGB-D modules: Astra+/Femto)" ON
+  VISIBLE_IF (WIN32 AND NOT ARM AND NOT WINRT) OR ( UNIX AND NOT APPLE AND NOT ANDROID)
+  VERIFY HAVE_OBSENSOR)
 
 # OpenCV build components
 # ===================================================

modules/videoio/CMakeLists.txt

@@ -235,6 +235,30 @@ if(TARGET ocv.3rdparty.android_native_camera)
   list(APPEND tgts ocv.3rdparty.android_native_camera)
 endif()
 
+if(TARGET ocv.3rdparty.obsensor)
+  list(APPEND videoio_srcs
+    ${CMAKE_CURRENT_LIST_DIR}/src/cap_obsensor_capture.cpp)
+  list(APPEND videoio_hdrs
+    ${CMAKE_CURRENT_LIST_DIR}/src/cap_obsensor_capture.hpp
+    ${CMAKE_CURRENT_LIST_DIR}/src/cap_obsensor/obsensor_stream_channel_interface.hpp)
+  if(HAVE_OBSENSOR_MSMF)
+    list(APPEND videoio_srcs
+      ${CMAKE_CURRENT_LIST_DIR}/src/cap_obsensor/obsensor_uvc_stream_channel.cpp
+      ${CMAKE_CURRENT_LIST_DIR}/src/cap_obsensor/obsensor_stream_channel_msmf.cpp)
+    list(APPEND videoio_hdrs
+      ${CMAKE_CURRENT_LIST_DIR}/src/cap_obsensor/obsensor_uvc_stream_channel.hpp
+      ${CMAKE_CURRENT_LIST_DIR}/src/cap_obsensor/obsensor_stream_channel_msmf.hpp)
+  elseif(HAVE_OBSENSOR_V4L2)
+    list(APPEND videoio_srcs
+      ${CMAKE_CURRENT_LIST_DIR}/src/cap_obsensor/obsensor_uvc_stream_channel.cpp
+      ${CMAKE_CURRENT_LIST_DIR}/src/cap_obsensor/obsensor_stream_channel_v4l2.cpp)
+    list(APPEND videoio_hdrs
+      ${CMAKE_CURRENT_LIST_DIR}/src/cap_obsensor/obsensor_uvc_stream_channel.hpp
+      ${CMAKE_CURRENT_LIST_DIR}/src/cap_obsensor/obsensor_stream_channel_v4l2.hpp)
+  endif()
+  list(APPEND tgts ocv.3rdparty.obsensor)
+endif()
+
 if(tgts STREQUAL "PRIVATE")
   set(tgts "")
 endif()

modules/videoio/cmake/detect_obsensor.cmake

@@ -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()

modules/videoio/cmake/init.cmake

@@ -22,6 +22,7 @@ add_backend("realsense" WITH_LIBREALSENSE)
 add_backend("ueye" WITH_UEYE)
 add_backend("ximea" WITH_XIMEA)
 add_backend("xine" WITH_XINE)
+add_backend("obsensor" WITH_OBSENSOR)
 
 add_backend("avfoundation" WITH_AVFOUNDATION)
 add_backend("ios" WITH_CAP_IOS)

modules/videoio/include/opencv2/videoio.hpp

@@ -123,6 +123,7 @@ enum VideoCaptureAPIs {
        CAP_INTEL_MFX    = 2300,         //!< Intel MediaSDK
        CAP_XINE         = 2400,         //!< XINE engine (Linux)
        CAP_UEYE         = 2500,         //!< uEye Camera API
+       CAP_OBSENSOR     = 2600,         //!< For Orbbec 3D-Sensor device/module (Astra+, Femto)
      };
 
 /** @brief cv::VideoCapture generic properties identifier.
@@ -654,6 +655,35 @@ enum { CAP_PROP_IMAGES_BASE = 18000,
 
 //! @} Images
 
+/** @name OBSENSOR (for Orbbec 3D-Sensor device/module )
+    @{
+*/
+//! OBSENSOR data given from image generator
+enum VideoCaptureOBSensorDataType{ 
+    CAP_OBSENSOR_DEPTH_MAP = 0, //!< Depth values in mm (CV_16UC1)
+    CAP_OBSENSOR_BGR_IMAGE = 1, //!< Data given from BGR stream generator
+    CAP_OBSENSOR_IR_IMAGE = 2   //!< Data given from IR stream generator(CV_16UC1)
+};
+
+//! OBSENSOR stream generator
+enum VideoCaptureOBSensorGenerators{ 
+    CAP_OBSENSOR_DEPTH_GENERATOR = 1 << 29,
+    CAP_OBSENSOR_IMAGE_GENERATOR = 1 << 28,
+    CAP_OBSENSOR_IR_GENERATOR    = 1 << 27,  
+    CAP_OBSENSOR_GENERATORS_MASK = CAP_OBSENSOR_DEPTH_GENERATOR + CAP_OBSENSOR_IMAGE_GENERATOR + CAP_OBSENSOR_IR_GENERATOR
+};
+
+//!OBSENSOR properties 
+enum VideoCaptureOBSensorProperties{
+    // INTRINSIC
+    CAP_PROP_OBSENSOR_INTRINSIC_FX=26001,
+    CAP_PROP_OBSENSOR_INTRINSIC_FY=26002,
+    CAP_PROP_OBSENSOR_INTRINSIC_CX=26003,
+    CAP_PROP_OBSENSOR_INTRINSIC_CY=26004,
+};
+
+//! @} OBSENSOR
+
 //! @} videoio_flags_others
 
 

modules/videoio/src/cap_interface.hpp

@@ -411,6 +411,8 @@ Ptr<IVideoCapture> createXINECapture(const std::string &filename);
 Ptr<IVideoCapture> createAndroidCapture_cam( int index );
 Ptr<IVideoCapture> createAndroidCapture_file(const std::string &filename);
 
+Ptr<IVideoCapture> create_obsensor_capture(int index);
+
 bool VideoCapture_V4L_waitAny(
         const std::vector<VideoCapture>& streams,
         CV_OUT std::vector<int>& ready,

modules/videoio/src/cap_obsensor/obsensor_stream_channel_interface.hpp

@@ -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

modules/videoio/src/cap_obsensor/obsensor_stream_channel_msmf.cpp

@@ -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, &current_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

modules/videoio/src/cap_obsensor/obsensor_stream_channel_msmf.hpp

@@ -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

modules/videoio/src/cap_obsensor/obsensor_stream_channel_v4l2.cpp

@@ -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

modules/videoio/src/cap_obsensor/obsensor_stream_channel_v4l2.hpp

@@ -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

modules/videoio/src/cap_obsensor/obsensor_uvc_stream_channel.cpp

@@ -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

modules/videoio/src/cap_obsensor/obsensor_uvc_stream_channel.hpp

@@ -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

modules/videoio/src/cap_obsensor_capture.cpp

@@ -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

modules/videoio/src/cap_obsensor_capture.hpp

@@ -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

modules/videoio/src/videoio_registry.cpp

@@ -53,7 +53,7 @@ namespace {
 /** Ordering guidelines:
 - modern optimized, multi-platform libraries: ffmpeg, gstreamer, Media SDK
 - platform specific universal SDK: WINRT, AVFOUNDATION, MSMF/DSHOW, V4L/V4L2
-- RGB-D: OpenNI/OpenNI2, REALSENSE
+- RGB-D: OpenNI/OpenNI2, REALSENSE, OBSENSOR
 - special OpenCV (file-based): "images", "mjpeg"
 - special camera SDKs, including stereo: other special SDKs: FIREWIRE/1394, XIMEA/ARAVIS/GIGANETIX/PVAPI(GigE)/uEye
 - other: XINE, gphoto2, etc
@@ -171,6 +171,11 @@ static const struct VideoBackendInfo builtin_backends[] =
 #endif
                            0)
 #endif
+
+#ifdef HAVE_OBSENSOR
+    DECLARE_STATIC_BACKEND(CAP_OBSENSOR, "OBSENSOR", MODE_CAPTURE_BY_INDEX, 0, create_obsensor_capture, 0)
+#endif
+
     // dropped backends: MIL, TYZX
 };
 

samples/cpp/videocapture_obsensor.cpp

@@ -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;
+}