add support for Orbbec Femto Mega RGB-D camera

artoolkitx · Feb 11, 2023 · 325fe7e · 325fe7e
1 parent 606c803
commit 325fe7e
Show file tree

Hide file tree

Showing 4 changed files with 40 additions and 7 deletions.
diff --git a/modules/videoio/src/cap_obsensor/obsensor_stream_channel_interface.hpp b/modules/videoio/src/cap_obsensor/obsensor_stream_channel_interface.hpp
@@ -36,6 +36,7 @@ namespace obsensor {
 #define OBSENSOR_CAM_VID 0x2bc5 // usb vid
 #define OBSENSOR_ASTRA2_PID 0x0660 // pid of Orbbec Astra 2 Camera
 #define OBSENSOR_GEMINI2_PID 0x0670 // pid of Orbbec Gemini 2 Camera
+#define OBSENSOR_FEMTO_MEGA_PID 0x0669 // pid of Orbbec Femto Mega Camera
 
 enum StreamType
 {

diff --git a/modules/videoio/src/cap_obsensor/obsensor_stream_channel_msmf.cpp b/modules/videoio/src/cap_obsensor/obsensor_stream_channel_msmf.cpp
@@ -499,7 +499,7 @@ STDMETHODIMP MSMFStreamChannel::OnEvent(DWORD /*sidx*/, IMFMediaEvent* /*event*/
 
 STDMETHODIMP MSMFStreamChannel::OnFlush(DWORD)
 {
-    if (streamState_ == STREAM_STARTING)
+    if (streamState_ != STREAM_STOPED)
     {
         std::unique_lock<std::mutex> lock(streamStateMutex_);
         streamState_ = STREAM_STOPED;

diff --git a/modules/videoio/src/cap_obsensor/obsensor_uvc_stream_channel.cpp b/modules/videoio/src/cap_obsensor/obsensor_uvc_stream_channel.cpp
@@ -338,6 +338,24 @@ bool IUvcStreamChannel::getProperty(int propId, uint8_t* recvData, uint32_t* rec
             *recvDataSize = sizeof(CameraParam);
             memcpy(recvData, &param, *recvDataSize);
         }
+        else if(OBSENSOR_FEMTO_MEGA_PID == devInfo_.pid){
+            // return default param
+            CameraParam param;
+            param.p0[0] = 748.370f;
+            param.p0[1] = 748.296f;
+            param.p0[2] = 634.670f;
+            param.p0[3] = 341.196f;
+            param.p1[0] = 374.185f;
+            param.p1[1] = 374.148f;
+            param.p1[2] = 317.335f;
+            param.p1[3] = 170.598f;
+            param.p6[0] = 1280;
+            param.p6[1] = 720;
+            param.p7[0] = 640;
+            param.p7[1] = 360;
+            *recvDataSize = sizeof(CameraParam);
+            memcpy(recvData, &param, *recvDataSize);
+        }
         else{
             rst &= setXu(2, OB_EXT_CMD5, sizeof(OB_EXT_CMD5));
             rst &= getXu(2, &rcvData, &rcvLen);

diff --git a/modules/videoio/src/cap_obsensor_capture.cpp b/modules/videoio/src/cap_obsensor_capture.cpp
@@ -34,8 +34,10 @@ VideoCapture_obsensor::VideoCapture_obsensor(int index) : isOpened_(false)
 {
     static const obsensor::StreamProfile colorProfile = { 640, 480, 30, obsensor::FRAME_FORMAT_MJPG };
     static const obsensor::StreamProfile depthProfile = {640, 480, 30, obsensor::FRAME_FORMAT_Y16};
-    static const obsensor::StreamProfile gemini2depthProfile = {1280, 800, 30, obsensor::FRAME_FORMAT_Y14};
-    static const obsensor::StreamProfile astra2depthProfile = {640, 480, 30, obsensor::FRAME_FORMAT_Y14};
+    static const obsensor::StreamProfile gemini2DepthProfile = {1280, 800, 30, obsensor::FRAME_FORMAT_Y14};
+    static const obsensor::StreamProfile astra2DepthProfile = {640, 480, 30, obsensor::FRAME_FORMAT_Y14};
+    static const obsensor::StreamProfile megaColorProfile = {1280, 720, 30, obsensor::FRAME_FORMAT_MJPG};
+    static const obsensor::StreamProfile megaDepthProfile = {640, 576, 30, obsensor::FRAME_FORMAT_Y16};
 
     streamChannelGroup_ = obsensor::getStreamChannelGroup(index);
     if (!streamChannelGroup_.empty())
@@ -46,11 +48,17 @@ VideoCapture_obsensor::VideoCapture_obsensor(int index) : isOpened_(false)
             switch (streamType)
             {
             case obsensor::OBSENSOR_STREAM_COLOR:
-                channel->start(colorProfile, [&](obsensor::Frame* frame) {
+            {
+                auto profile = colorProfile;
+                if(OBSENSOR_FEMTO_MEGA_PID == channel->getPid()){
+                    profile = megaColorProfile;
+                }
+                channel->start(profile, [&](obsensor::Frame* frame) {
                     std::unique_lock<std::mutex> lk(frameMutex_);
                     colorFrame_ = Mat(1, frame->dataSize, CV_8UC1, frame->data).clone();
                     frameCv_.notify_all();
                 });
+            }
                 break;
             case obsensor::OBSENSOR_STREAM_DEPTH:
             {
@@ -59,11 +67,13 @@ VideoCapture_obsensor::VideoCapture_obsensor(int index) : isOpened_(false)
 
                 obsensor::StreamProfile profile = depthProfile;
                 if(OBSENSOR_GEMINI2_PID == channel->getPid()){
-                    profile = gemini2depthProfile;
+                    profile = gemini2DepthProfile;
                 }
                 else if(OBSENSOR_ASTRA2_PID == channel->getPid()){
-
-                    profile = astra2depthProfile;
+                    profile = astra2DepthProfile;
+                }
+                else if(OBSENSOR_FEMTO_MEGA_PID == channel->getPid()){
+                    profile = megaDepthProfile;
                 }
 
                 channel->start(profile, [&](obsensor::Frame* frame) {
@@ -127,6 +137,10 @@ bool VideoCapture_obsensor::retrieveFrame(int outputType, OutputArray frame)
                 grabbedDepthFrame_ = grabbedDepthFrame_*0.8;
                 grabbedDepthFrame_.copyTo(frame);
             }
+            else if(OBSENSOR_FEMTO_MEGA_PID == streamChannelGroup_.front()->getPid()){
+                Rect rect(0, 0, 640, 360);
+                grabbedDepthFrame_(rect).copyTo(frame);
+            }
             else{
                 grabbedDepthFrame_.copyTo(frame);
             }