Opencv+Kinect2.0抠图

不知道为什么，这个用c++写的运行起来比c#写的还要慢，用了OpenMP也只是勉强能看。
有空再改改。

#include "opencv2/core.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/videoio.hpp"
#include <iostream>
#include <Kinect.h>  
#include<Kinect.Face.h>
#include <Kinect.VisualGestureBuilder.h>
#pragma comment ( lib, "kinect20.lib" )  
#pragma comment ( lib, "Kinect20.face.lib" )  
#pragma comment ( lib, "Kinect20.VisualGestureBuilder.lib" )  

using namespace cv;
using namespace std;

template<class Interface>
inline void SafeRelease(Interface *& pInterfaceToRelease)
{
    if (pInterfaceToRelease != NULL)
    {
        pInterfaceToRelease->Release();
        pInterfaceToRelease = NULL;
    }
}

int main()
{
    HRESULT hResult;
    setUseOptimized(true);

    IKinectSensor *kinect;
    GetDefaultKinectSensor(&kinect);
    kinect->Open();

    IBodyIndexFrameSource *bodyindexframesource;
    kinect->get_BodyIndexFrameSource(&bodyindexframesource);
    IBodyIndexFrameReader *bodyindexframereader;
    bodyindexframesource->OpenReader(&bodyindexframereader);
    IColorFrameSource*colorframesource;
    kinect->get_ColorFrameSource(&colorframesource);
    IColorFrameReader*colorframereader;
    colorframesource->OpenReader(&colorframereader);
    ICoordinateMapper*coordinate;
    kinect->get_CoordinateMapper(&coordinate);
    IDepthFrameSource*depthframesource;
    kinect->get_DepthFrameSource(&depthframesource);
    IDepthFrameReader*depthframereader;
    depthframesource->OpenReader(&depthframereader);

    IFrameDescription*nfd;
    depthframesource->get_FrameDescription(&nfd);
    int nwidth = 0;
    int nheight = 0;
    nfd->get_Width(&nwidth);
    nfd->get_Height(&nheight);

    IFrameDescription *cfd;
    colorframesource->get_FrameDescription(&cfd);
    int cwidth = 0;
    int cheight = 0;
    cfd->get_Width(&cwidth);
    cfd->get_Height(&cheight);

    vector<DepthSpacePoint>depthspacepoints(cwidth*cheight);
    Mat img(cheight, cwidth, CV_8UC4);

    while (1)
    {
        UINT nBodyIndexBufferSize = 0;
        BYTE *pBodyIndexBuffer = NULL;
        IBodyIndexFrame*bodyindexframe;
        hResult = bodyindexframereader->AcquireLatestFrame(&bodyindexframe);
        if (SUCCEEDED(hResult))
        {
            hResult = bodyindexframe->AccessUnderlyingBuffer(&nBodyIndexBufferSize, &pBodyIndexBuffer);
        }


        if (SUCCEEDED(hResult))
        {
            UINT nDepthBufferSize = 0;
            UINT16 *pDepthBuffer = NULL;
            IDepthFrame*depthframe;
            hResult = depthframereader->AcquireLatestFrame(&depthframe);
            if (SUCCEEDED(hResult))
            {
                hResult = depthframe->AccessUnderlyingBuffer(&nDepthBufferSize, &pDepthBuffer);
            }


            if (SUCCEEDED(hResult))
            {
                UINT nColorBufferSize = 0;
                RGBQUAD *pColorBuffer = NULL;
                IColorFrame*colorframe;
                hResult = colorframereader->AcquireLatestFrame(&colorframe);
                if (SUCCEEDED(hResult))
                {
                    hResult = colorframe->CopyConvertedFrameDataToArray(cheight*cwidth * 4, reinterpret_cast<BYTE*>(img.data), ColorImageFormat::ColorImageFormat_Bgra);
                    if (SUCCEEDED(hResult))
                    {
                        hResult = colorframe->AccessRawUnderlyingBuffer(&nColorBufferSize, reinterpret_cast<BYTE**>(&pColorBuffer));
                    }
                }

                if (SUCCEEDED(hResult))
                {

                    coordinate->MapColorFrameToDepthSpace(nwidth * nheight, (UINT16*)pDepthBuffer, cwidth * cheight, &depthspacepoints[0]);
#pragma omp parallel for
                    for (int i = 0; i < cheight; i++)
                        for (int j = 0; j < cwidth; j++)
                        {
                            int k = i*cwidth + j;
                            DepthSpacePoint p = depthspacepoints[k];

                            int depthX = (int)(p.X + 0.5f);
                            int depthY = (int)(p.Y + 0.5f);

                            if ((depthX >= 0 && depthX < nwidth) && (depthY >= 0 && depthY < nheight))
                            {
                                BYTE player = pBodyIndexBuffer[depthX + (depthY * nwidth)];
                                if (player == 0xff)
                                {
                                    img.at <Vec4b>(i,j) = Vec4b(0, 0, 0, 255);
                                }
                            }
                            else
                                img.at <Vec4b>(i, j) = Vec4b(0, 0, 0, 255);
                        }
                }
                SafeRelease(colorframe);
            }

            SafeRelease(depthframe);
        }

        SafeRelease(bodyindexframe);

        imshow("aaa", img);
        if (waitKey(34) > 0)
        {
            break;
        }
    }
    SafeRelease(bodyindexframesource);
    SafeRelease(bodyindexframereader);
    SafeRelease(nfd);
    if (kinect) 
    {
        kinect->Close();
    }
    SafeRelease(kinect);

    return 0;
}

运行图：