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();

}