Subversion Repositories Code-Repo

#include "OpenNI.h"

#define CHECK_RC(rc, what)                                                                              \
if (rc != XN_STATUS_OK) {                                                                               \
        QString error = QString(QString(what) + "failed: ");            \
        error += QString(xnGetStatusString(rc));                                        \
        emit setStatusString(error);                                                            \
        exit(rc);                                                                                                       \
        return;                                                                                                         \
}

using namespace xn;

extern QSemaphore processorBuffer;

XnBool OpenNI::fileExists(const char *fn) {
        XnBool exists;
        xnOSDoesFileExist(fn, &exists);
        return exists;
}

OpenNI::OpenNI(QObject *parent)
: QThread(parent) {

        moveToThread(this);
}

OpenNI::~OpenNI() {

}

void OpenNI::run() {

        XnStatus nRetVal = XN_STATUS_OK;
        Context context;
        ScriptNode scriptNode;
        EnumerationErrors errors;

        // Check if the config file exists
        const char *fn = NULL;
        if (fileExists(CONFIG_XML_PATH)) fn = CONFIG_XML_PATH;
        else if (fileExists(CONFIG_XML_PATH_LOCAL)) fn = CONFIG_XML_PATH_LOCAL;
        else {
                QString error = QString("Could not find " + QString(CONFIG_XML_PATH) + " nor " + QString(CONFIG_XML_PATH_LOCAL));
                emit setStatusString(error);
                exit(XN_STATUS_ERROR);
                return;
        }
        // Load the config file
        nRetVal = context.InitFromXmlFile(fn, scriptNode, &errors);

        // Check to see if a sensor is connected
        if (nRetVal == XN_STATUS_NO_NODE_PRESENT) {
                XnChar strError[1024];
                errors.ToString(strError, 1024);
                emit setStatusString(QString(strError));
                exit(nRetVal);
                return;
        } else if (nRetVal != XN_STATUS_OK) {
                QString error = QString("Open failed: " + QString(xnGetStatusString(nRetVal)));
                emit setStatusString(error);
                exit(nRetVal);
                return;
        }

        // Connect to the attached sensor
        DepthGenerator depth;
        nRetVal = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth);
        CHECK_RC(nRetVal, "Find depth generator");

        // Pull FPS and depth data
        XnFPSData xnFPS;
        nRetVal = xnFPSInit(&xnFPS, 180);
        CHECK_RC(nRetVal, "FPS Init");

        DepthMetaData depthMD;

        short min = 9999, max = 0;

        // If this point is reached, notify that the sensor is ready
        emit sensorConnected();

        while (1) {
                // Ensure that the processing queue isnt too long
                processorBuffer.acquire(1);

                // Wait for an updated frame from the sensor
                nRetVal = context.WaitOneUpdateAll(depth);
                if (nRetVal != XN_STATUS_OK) {
                        QString error = QString("UpdateData failed: " + QString(xnGetStatusString(nRetVal)));
                        emit setStatusString(error);
                        break;
                }

                // Mark the frame (for determining FPS)
                xnFPSMarkFrame(&xnFPS);

                // Grab the depth data
                depth.GetMetaData(depthMD);

                // Grab the size and FOV of the sensor
                int X = depthMD.XRes();
                int Y = depthMD.YRes();

                XnFieldOfView fov;
                depth.GetFieldOfView(fov);

                emit setFOV(fov.fHFOV, fov.fVFOV);

                // Show the size/FOV/FPS/frame
                QString status = "Res: " + QString::number(X) + " x " + QString::number(Y);
                //status += " / FoV (rad): " + QString::number(fov.fHFOV) + " (H) " + QString::number(fov.fVFOV) + " (V)";
                min = qMin(min, (short)depthMD(depthMD.XRes() / 2, depthMD.YRes() / 2));
                max = qMax(max, (short)depthMD(depthMD.XRes() / 2, depthMD.YRes() / 2));

                status += " / Center: " + QString::number(depthMD(depthMD.XRes() / 2, depthMD.YRes() / 2));
                status += " / Min: " + QString::number(min) + " / Max: " + QString::number(max);
                emit setStatusString(status);
                emit setFPSString(QString::number(xnFPSCalc(&xnFPS)));
                emit setFrameString(QString::number(depthMD.FrameID()));

                // Allocate a new cv::Mat
                cv::Mat depthData = cv::Mat(Y_RES, X_RES, CV_16UC1);

                // Save the depth data (mirrored horizontally)
                ushort *p;
                for (int y = 0; y < Y; ++y) {
                        p = depthData.ptr<ushort>(y);
                        for (int x = 0; x < X; ++x) {
                                short pixel = depthMD.Data()[(y * X) + (X - x - 1)];
                                p[x] = pixel;
                        }
                }

                // Send the depth data for processing
                emit processDepthData(depthData);
        }

        depth.Release();
        scriptNode.Release();
        context.Release();
}