Interfacing Intel RealSense F200 with OpenCV

Datetime:2016-08-22 23:39:14          Topic: OpenCV           Share

RealSense from Intel is an interesting technology that brings 3D cameras into the mainstream.Although the RealSense SDK provides a good starting point for many applications, some users would prefer to have a bit more control over the images. In this post, I’ll describe how to access the raw streams from an Intel RealSense F200 camera, and how to convert those images into OpenCV cv::Mat objects.

Here is the video with all the explanations:

And here are the files used in the video:

01-alignedSingleThreaded.cpp:

#include <pxcsensemanager.h>
#include <opencv2/opencv.hpp>

PXCSenseManager *pxcSenseManager;


PXCImage * CVMat2PXCImage(cv::Mat cvImage)
{
    PXCImage::ImageInfo iinfo;
    memset(&iinfo,0,sizeof(iinfo));
    iinfo.width=cvImage.cols;
    iinfo.height=cvImage.rows;

    PXCImage::PixelFormat format;
    int type = cvImage.type();
    if(type == CV_8UC1)
        format = PXCImage::PIXEL_FORMAT_Y8;
    else if(type == CV_8UC3)
        format = PXCImage::PIXEL_FORMAT_RGB24;
    else if(type == CV_32FC1)
        format = PXCImage::PIXEL_FORMAT_DEPTH_F32;

    iinfo.format = format;


    PXCImage *pxcImage = pxcSenseManager->QuerySession()->CreateImage(&iinfo);

    PXCImage::ImageData data;
    pxcImage->AcquireAccess(PXCImage::ACCESS_WRITE, format, &data);

    data.planes[0] = cvImage.data;

    pxcImage->ReleaseAccess(&data);
    return pxcImage;
}


cv::Mat PXCImage2CVMat(PXCImage *pxcImage, PXCImage::PixelFormat format)
{
    PXCImage::ImageData data;
    pxcImage->AcquireAccess(PXCImage::ACCESS_READ, format, &data);

    int width = pxcImage->QueryInfo().width;
    int height = pxcImage->QueryInfo().height;
    if(!format)
        format = pxcImage->QueryInfo().format;

    int type;
    if(format == PXCImage::PIXEL_FORMAT_Y8)
        type = CV_8UC1;
    else if(format == PXCImage::PIXEL_FORMAT_RGB24)
        type = CV_8UC3;
    else if(format == PXCImage::PIXEL_FORMAT_DEPTH_F32)
        type = CV_32FC1;
    else if(format == PXCImage::PIXEL_FORMAT_DEPTH)
        type = CV_16UC1;

    cv::Mat ocvImage = cv::Mat(cv::Size(width, height), type, data.planes[0]);


    pxcImage->ReleaseAccess(&data);
    return ocvImage;
}

int main(int argc, char* argv[])
{
    //Define some parameters for the camera
    cv::Size frameSize = cv::Size(640, 480);
    float frameRate = 60;

    //Create the OpenCV windows and images
    cv::namedWindow("IR", cv::WINDOW_NORMAL);
    cv::namedWindow("Color", cv::WINDOW_NORMAL);
    cv::namedWindow("Depth", cv::WINDOW_NORMAL);
    cv::Mat frameIR = cv::Mat::zeros(frameSize, CV_8UC1);
    cv::Mat frameColor = cv::Mat::zeros(frameSize, CV_8UC3);
    cv::Mat frameDepth = cv::Mat::zeros(frameSize, CV_8UC1);


    //Initialize the RealSense Manager
    pxcSenseManager = PXCSenseManager::CreateInstance();

    //Enable the streams to be used
    pxcSenseManager->EnableStream(PXCCapture::STREAM_TYPE_IR, frameSize.width, frameSize.height, frameRate);
    pxcSenseManager->EnableStream(PXCCapture::STREAM_TYPE_COLOR, frameSize.width, frameSize.height, frameRate);
    pxcSenseManager->EnableStream(PXCCapture::STREAM_TYPE_DEPTH, frameSize.width, frameSize.height, frameRate);

    //Initialize the pipeline
    pxcSenseManager->Init();

    bool keepRunning = true;
    while(keepRunning)
    {
        //Acquire all the frames from the camera
        pxcSenseManager->AcquireFrame();
        PXCCapture::Sample *sample = pxcSenseManager->QuerySample();

        //Convert each frame into an OpenCV image
        frameIR = PXCImage2CVMat(sample->ir, PXCImage::PIXEL_FORMAT_Y8);
        frameColor = PXCImage2CVMat(sample->color, PXCImage::PIXEL_FORMAT_RGB24);
        cv::Mat frameDepth_u16 = PXCImage2CVMat(sample->depth, PXCImage::PIXEL_FORMAT_DEPTH);
        frameDepth_u16.convertTo(frameDepth, CV_8UC1);

        cv::Mat frameDisplay;
        cv::equalizeHist(frameDepth, frameDisplay);

        //Display the images
        cv::imshow("IR", frameIR);
        cv::imshow("Color", frameColor);
        cv::imshow("Depth", frameDisplay);

        //Check for user input
        int key = cv::waitKey(1);
        if(key == 27)            
            keepRunning = false;

        //Release the memory from the frames
        pxcSenseManager->ReleaseFrame();
    }

    //Release the memory from the RealSense manager
    pxcSenseManager->Release();

    return 0;
}

02-unalignedSingleThreaded.cpp:

#include <pxcsensemanager.h>
#include <opencv2/opencv.hpp>

cv::Mat PXCImage2CVMat(PXCImage *pxcImage, PXCImage::PixelFormat format)
{
    PXCImage::ImageData data;
    pxcImage->AcquireAccess(PXCImage::ACCESS_READ, format, &data);

    int width = pxcImage->QueryInfo().width;
    int height = pxcImage->QueryInfo().height;

    if(!format)
        format = pxcImage->QueryInfo().format;

    int type;
    if(format == PXCImage::PIXEL_FORMAT_Y8)
        type = CV_8UC1;
    else if(format == PXCImage::PIXEL_FORMAT_RGB24)
        type = CV_8UC3;
    else if(format == PXCImage::PIXEL_FORMAT_DEPTH_F32)
        type = CV_32FC1;

    cv::Mat ocvImage = cv::Mat(cv::Size(width, height), type, data.planes[0]);

    pxcImage->ReleaseAccess(&data);
    return ocvImage;
}

int main(int argc, char* argv[])
{
    //Define some parameters for the camera
    cv::Size frameSize = cv::Size(640, 480);
    float frameRate = 60;

    //Create the OpenCV windows and images
    cv::namedWindow("IR", cv::WINDOW_NORMAL);
    cv::namedWindow("Color", cv::WINDOW_NORMAL);
    cv::namedWindow("Depth", cv::WINDOW_NORMAL);
    cv::Mat frameIR = cv::Mat::zeros(frameSize, CV_8UC1);
    cv::Mat frameColor = cv::Mat::zeros(frameSize, CV_8UC3);
    cv::Mat frameDepth = cv::Mat::zeros(frameSize, CV_8UC1);


    //Initialize the RealSense Manager
    PXCSenseManager *pxcSenseManager = PXCSenseManager::CreateInstance();

    //Enable the streams to be used
    pxcSenseManager->EnableStream(PXCCapture::STREAM_TYPE_IR, frameSize.width, frameSize.height, frameRate);
    pxcSenseManager->EnableStream(PXCCapture::STREAM_TYPE_COLOR, frameSize.width, frameSize.height, frameRate);
    pxcSenseManager->EnableStream(PXCCapture::STREAM_TYPE_DEPTH, frameSize.width, frameSize.height, frameRate);

    //Initialize the pipeline
    pxcSenseManager->Init();

    bool keepRunning = true;
    while(keepRunning)
    {
        //Acquire any frame from the camera
        pxcSenseManager->AcquireFrame(false);
        PXCCapture::Sample *sample = pxcSenseManager->QuerySample();

        //Convert each frame into an OpenCV image
        //You need to make sure that the image is there first
        if(sample->ir)
            frameIR = PXCImage2CVMat(sample->ir, PXCImage::PIXEL_FORMAT_Y8);
        if(sample->color)
            frameColor = PXCImage2CVMat(sample->color, PXCImage::PIXEL_FORMAT_RGB24);
        if(sample->depth)
            PXCImage2CVMat(sample->depth, PXCImage::PIXEL_FORMAT_DEPTH_F32).convertTo(frameDepth, CV_8UC1);

        //Display the images
        cv::imshow("IR", frameIR);
        cv::imshow("Color", frameColor);
        cv::imshow("Depth", frameDepth);

        //Check for user input
        int key = cv::waitKey(1);
        if(key == 27)
            keepRunning = false;

        //Release the memory from the frames
        pxcSenseManager->ReleaseFrame();
    }

    //Release the memory from the RealSense manager
    pxcSenseManager->Release();

    return 0;
}

03-unalignedMultiThreaded.cpp:

#include <pxcsensemanager.h>
#include <iostream>
#include <opencv2/opencv.hpp>

cv::Mat frameIR;
cv::Mat frameColor;
cv::Mat frameDepth;
cv::Mutex framesMutex;


cv::Mat PXCImage2CVMat(PXCImage *pxcImage, PXCImage::PixelFormat format)
{
    PXCImage::ImageData data;
    pxcImage->AcquireAccess(PXCImage::ACCESS_READ, format, &data);

    int width = pxcImage->QueryInfo().width;
    int height = pxcImage->QueryInfo().height;

    if(!format)
        format = pxcImage->QueryInfo().format;

    int type;
    if(format == PXCImage::PIXEL_FORMAT_Y8)
        type = CV_8UC1;
    else if(format == PXCImage::PIXEL_FORMAT_RGB24)
        type = CV_8UC3;
    else if(format == PXCImage::PIXEL_FORMAT_DEPTH_F32)
        type = CV_32FC1;

    cv::Mat ocvImage = cv::Mat(cv::Size(width, height), type, data.planes[0]);

    pxcImage->ReleaseAccess(&data);
    return ocvImage;
}



class FramesHandler:public PXCSenseManager::Handler
{
public:
    virtual pxcStatus PXCAPI OnNewSample(pxcUID, PXCCapture::Sample *sample)
    {
            framesMutex.lock();
                if(sample->ir)
                    frameIR = PXCImage2CVMat(sample->ir, PXCImage::PIXEL_FORMAT_Y8);
                if(sample->color)
                    frameColor = PXCImage2CVMat(sample->color, PXCImage::PIXEL_FORMAT_RGB24);
                if(sample->depth)
                    PXCImage2CVMat(sample->depth, PXCImage::PIXEL_FORMAT_DEPTH_F32).convertTo(frameDepth, CV_8UC1);
            framesMutex.unlock();
    return PXC_STATUS_NO_ERROR;
    }
};


int main(int argc, char* argv[])
{
    cv::Size frameSize = cv::Size(640, 480);
    float frameRate = 60;

    cv::namedWindow("IR", cv::WINDOW_NORMAL);
    cv::namedWindow("Color", cv::WINDOW_NORMAL);
    cv::namedWindow("Depth", cv::WINDOW_NORMAL);
    frameIR = cv::Mat::zeros(frameSize, CV_8UC1);
    frameColor = cv::Mat::zeros(frameSize, CV_8UC3);
    frameDepth = cv::Mat::zeros(frameSize, CV_8UC1);

    PXCSenseManager *pxcSenseManager = PXCSenseManager::CreateInstance();

    //Enable the streams to be used
    pxcSenseManager->EnableStream(PXCCapture::STREAM_TYPE_IR, frameSize.width, frameSize.height, frameRate);
    pxcSenseManager->EnableStream(PXCCapture::STREAM_TYPE_COLOR, frameSize.width, frameSize.height, frameRate);
    pxcSenseManager->EnableStream(PXCCapture::STREAM_TYPE_DEPTH, frameSize.width, frameSize.height, frameRate);

    FramesHandler handler;
    pxcSenseManager->Init(&handler);
    pxcSenseManager->StreamFrames(false);

    //Local images for display
    cv::Mat displayIR = frameIR.clone();
    cv::Mat displayColor = frameColor.clone();
    cv::Mat displayDepth = frameDepth.clone();

    bool keepRunning = true;
    while(keepRunning)
    {
        framesMutex.lock();
            displayIR = frameIR.clone();
            displayColor = frameColor.clone();
            displayDepth = frameDepth.clone();
        framesMutex.unlock();

        cv::imshow("IR", displayIR);
        cv::imshow("Color", displayColor);
        cv::imshow("Depth", displayDepth);

        int key = cv::waitKey(1);
        if(key == 27)
            keepRunning = false;
    }
    //Stop the frame acqusition thread
    pxcSenseManager->Close();

    pxcSenseManager->Release();


    return 0;
}

04-alignedMultiThreaded.cpp:

#include <pxcsensemanager.h>
#include <iostream>
#include <opencv2/opencv.hpp>

cv::Mat frameIR;
cv::Mat frameColor;
cv::Mat frameDepth;
cv::Mutex framesMutex;


cv::Mat PXCImage2CVMat(PXCImage *pxcImage, PXCImage::PixelFormat format)
{
    PXCImage::ImageData data;
    pxcImage->AcquireAccess(PXCImage::ACCESS_READ, format, &data);

    int width = pxcImage->QueryInfo().width;
    int height = pxcImage->QueryInfo().height;

    if(!format)
        format = pxcImage->QueryInfo().format;

    int type;
    if(format == PXCImage::PIXEL_FORMAT_Y8)
        type = CV_8UC1;
    else if(format == PXCImage::PIXEL_FORMAT_RGB24)
        type = CV_8UC3;
    else if(format == PXCImage::PIXEL_FORMAT_DEPTH_F32)
        type = CV_32FC1;

    cv::Mat ocvImage = cv::Mat(cv::Size(width, height), type, data.planes[0]);

    pxcImage->ReleaseAccess(&data);
    return ocvImage;
}



class FramesHandler:public PXCSenseManager::Handler
{
public:
    virtual pxcStatus PXCAPI OnNewSample(pxcUID, PXCCapture::Sample *sample)
    {
            framesMutex.lock();
                frameIR = PXCImage2CVMat(sample->ir, PXCImage::PIXEL_FORMAT_Y8);
                frameColor = PXCImage2CVMat(sample->color, PXCImage::PIXEL_FORMAT_RGB24);
                PXCImage2CVMat(sample->depth, PXCImage::PIXEL_FORMAT_DEPTH_F32).convertTo(frameDepth, CV_8UC1);
            framesMutex.unlock();
    return PXC_STATUS_NO_ERROR;
    }
};


int main(int argc, char* argv[])
{
    cv::Size frameSize = cv::Size(640, 480);
    float frameRate = 60;

    cv::namedWindow("IR", cv::WINDOW_NORMAL);
    cv::namedWindow("Color", cv::WINDOW_NORMAL);
    cv::namedWindow("Depth", cv::WINDOW_NORMAL);
    frameIR = cv::Mat::zeros(frameSize, CV_8UC1);
    frameColor = cv::Mat::zeros(frameSize, CV_8UC3);
    frameDepth = cv::Mat::zeros(frameSize, CV_8UC1);

    PXCSenseManager *pxcSenseManager = PXCSenseManager::CreateInstance();

    //Enable the streams to be used
    PXCVideoModule::DataDesc ddesc={};
    ddesc.deviceInfo.streams = PXCCapture::STREAM_TYPE_IR | PXCCapture::STREAM_TYPE_COLOR | PXCCapture::STREAM_TYPE_DEPTH;

    pxcSenseManager->EnableStreams(&ddesc);

    FramesHandler handler;
    pxcSenseManager->Init(&handler);
    pxcSenseManager->StreamFrames(false);

    //Local images for display
    cv::Mat displayIR = frameIR.clone();
    cv::Mat displayColor = frameColor.clone();
    cv::Mat displayDepth = frameDepth.clone();

    bool keepRunning = true;
    while(keepRunning)
    {
        framesMutex.lock();
            displayIR = frameIR.clone();
            displayColor = frameColor.clone();
            displayDepth = frameDepth.clone();
        framesMutex.unlock();

        cv::imshow("IR", displayIR);
        cv::imshow("Color", displayColor);
        cv::imshow("Depth", displayDepth);

        int key = cv::waitKey(1);
        if(key == 27)
            keepRunning = false;       
    }
    //Stop the frame acqusition thread
    pxcSenseManager->Close();

    pxcSenseManager->Release();


    return 0;
}




About List