openFrameworks + OpenNI Sample

testApp.cpp

#include "testApp.h"

#include <iostream>
#include <XnCppWrapper.h>

#define MAX_DEPTH 10000


void errorCheck(XnStatus status) {
    if (status != XN_STATUS_OK) {
        throw std::runtime_error(xnGetStatusString(status));
    }	
}

float *createHist(xn::DepthMetaData *depthMetaData, XnUInt numPixels) {
    static float depthHist[MAX_DEPTH];
    xnOSMemSet(depthHist, 0, MAX_DEPTH*sizeof(float));

    const XnDepthPixel* depthPixels = depthMetaData->Data();		    

    unsigned int numberOfPoints = 0;
    for (XnUInt i = 0; i < numPixels; ++i, ++depthPixels) {
        if (*depthPixels != 0) {
            depthHist[*depthPixels]++;
            numberOfPoints++;
        }
    }
    for (int nIndex=1; nIndex<MAX_DEPTH; nIndex++) {
        depthHist[nIndex] += depthHist[nIndex-1];
    }
    if (numberOfPoints) {
        for (int nIndex=1; nIndex<MAX_DEPTH; nIndex++) {
            depthHist[nIndex] = (unsigned int)(256 * (1.0f - (depthHist[nIndex] / numberOfPoints)));
        }
    }
    return depthHist;
}


//--------------------------------------------------------------
void testApp::setup(){

    ofSetLogLevel(OF_LOG_NOTICE);
    ofSetFrameRate(60);

    try {
        // OpenNIコンテキストの作成
        xn::EnumerationErrors ctxErrors;
        static string configFilePath = ofFilePath::getCurrentWorkingDirectory() +
            "../Resources/config.xml";
        XnStatus rc = openNIContext.InitFromXmlFile(configFilePath.c_str(), &ctxErrors);
        if (rc != XN_STATUS_OK) {
            XnChar strError[1024];
            ctxErrors.ToString(strError, 1024);
            throw std::runtime_error(strError);
        }

        {
            // DepthGeneratorの取得
            rc = openNIContext.FindExistingNode(XN_NODE_TYPE_DEPTH, depthGen);
            errorCheck(rc);

            XnMapOutputMode mapMode;
            depthGen.GetMapOutputMode(mapMode);

            int width = mapMode.nXRes;
            int height = mapMode.nYRes;
            int numPixels = width * height;

            // グレースケール用ピクセルデータ
            grayPixels = new unsigned char[numPixels];
            xnOSMemSet(grayPixels, 0, numPixels * sizeof(unsigned char));

            grayImage.allocate(width, height);
        }

        {
            // ImageGeneratorの取得
            rc = openNIContext.FindExistingNode(XN_NODE_TYPE_IMAGE, imageGen);
            errorCheck(rc);

            XnMapOutputMode mapMode;
            imageGen.GetMapOutputMode(mapMode);

            colorImage.allocate(mapMode.nXRes, mapMode.nYRes);
        }


        {
            // AudioGeneratorの取得
            rc = openNIContext.FindExistingNode(XN_NODE_TYPE_AUDIO, audioGen);
            errorCheck(rc);            
        }

    } catch (std::exception& ex) {
        ofLog(OF_LOG_WARNING, ex.what());
        ofLog(OF_LOG_WARNING, "EXIT by initializing error");
        exit();
    }

}

//--------------------------------------------------------------
void testApp::update(){

    if (depthGen.IsNewDataAvailable()) {
        // 深度データの更新
        depthGen.WaitAndUpdateData();

        xn::DepthMetaData depthMetaData;
        depthGen.GetMetaData(depthMetaData);

        XnMapOutputMode mapMode;
        depthGen.GetMapOutputMode(mapMode);

        int width = mapMode.nXRes;
        int height = mapMode.nYRes;
        int numPixels = width * height;

        float* depthHist = createHist(&depthMetaData, numPixels);

        const XnDepthPixel* depthPixels = depthMetaData.Data();
        for (XnUInt i = 0; i < numPixels; i++, ++depthPixels) {
            grayPixels[i] = depthHist[*depthPixels];
        }

        grayImage.setFromPixels(grayPixels, width, height);        
    }

    if (imageGen.IsNewDataAvailable()) {
        // ビデオカメラ画像の更新
        imageGen.WaitAndUpdateData();

        xn::ImageMetaData imageMetaData;
        imageGen.GetMetaData(imageMetaData);

        colorImage.setFromPixels((unsigned char*)imageMetaData.RGB24Data(), colorImage.width, colorImage.height);

    }

    if (audioGen.IsNewDataAvailable()) {
        // オーディオの更新
        audioGen.WaitAndUpdateData();
        ofLog(OF_LOG_NOTICE, "Audio data size:" + ofToString(audioGen.GetDataSize()));

        // doSomething
    }
}

//--------------------------------------------------------------
void testApp::draw(){
    ofScale(0.8, 0.8);
    grayImage.draw(0, 0);
    colorImage.draw(640, 0);
}

//--------------------------------------------------------------
void testApp::keyPressed(int key){

}

//--------------------------------------------------------------
void testApp::keyReleased(int key){

}

//--------------------------------------------------------------
void testApp::mouseMoved(int x, int y ){

}

//--------------------------------------------------------------
void testApp::mouseDragged(int x, int y, int button){

}

//--------------------------------------------------------------
void testApp::mousePressed(int x, int y, int button){

}

//--------------------------------------------------------------
void testApp::mouseReleased(int x, int y, int button){

}

//--------------------------------------------------------------
void testApp::windowResized(int w, int h){

}

//--------------------------------------------------------------
void testApp::gotMessage(ofMessage msg){

}

//--------------------------------------------------------------
void testApp::dragEvent(ofDragInfo dragInfo){ 

}

void testApp::exit() {
    openNIContext.Shutdown();
    ofLog(OF_LOG_WARNING, "APPLICATION EXIT");
}

testApp.h

#pragma once

#include "ofMain.h"
#include "ofxOpenCv.h"
#include <XnCppWrapper.h>

class testApp : public ofBaseApp{
    
public:
    void setup();
    void update();
    void draw();
    void exit();
    
    void keyPressed  (int key);
    void keyReleased(int key);
    void mouseMoved(int x, int y );
    void mouseDragged(int x, int y, int button);
    void mousePressed(int x, int y, int button);
    void mouseReleased(int x, int y, int button);
    void windowResized(int w, int h);
    void dragEvent(ofDragInfo dragInfo);
    void gotMessage(ofMessage msg);
    
    xn::Context openNIContext;
    xn::DepthGenerator depthGen;
    xn::ImageGenerator imageGen;
    xn::AudioGenerator audioGen;
    
    unsigned char *grayPixels;
    unsigned char *imagePixels;
    
    ofxCvGrayscaleImage 	grayImage;
    ofxCvColorImage         colorImage;
    
};