예제 #1
0
// -----------------------------------------------------------------------------------------------------
//  generateFrame
// -----------------------------------------------------------------------------------------------------
bool CameraDevice::generateFrame(IplImage* imgRGB, IplImage* imgDepth)
{
    XnStatus nRetVal = XN_STATUS_OK;
	const XnDepthPixel* pDepthMap = NULL;
	const XnRGB24Pixel* pImageMap = NULL;
	
	xnFPSMarkFrame(&g_xnFPS);
	nRetVal = g_context.WaitAndUpdateAll();
	if (nRetVal==XN_STATUS_OK)
	{
		g_depth.GetMetaData(g_depthMD);
		g_image.GetMetaData(g_imageMD);

		pDepthMap = g_depthMD.Data();
		pImageMap = g_image.GetRGB24ImageMap();

		printf("Frame %02d (%dx%d) Depth at middle point: %u. FPS: %f\r",
				g_depthMD.FrameID(),
				g_depthMD.XRes(),
				g_depthMD.YRes(),
				g_depthMD(g_depthMD.XRes()/2, g_depthMD.YRes()/2),
				xnFPSCalc(&g_xnFPS));

		// convert to OpenCV buffers
		convertImageRGB(pImageMap, imgRGB);
		convertImageDepth(pDepthMap, imgDepth);

		return true;
	}
	return false;
}
XnStatus prepare(char useScene, char useDepth, char useHistogram)
{
//TODO handle possible failures!
	if (useDepth)
	{
		mDepthGen.GetMetaData(depthMD);
		nXRes = depthMD.XRes();
		nYRes = depthMD.YRes();

		pDepth = depthMD.Data();

		if (useHistogram)
		{
			calcHist();

			// rewind the pointer
			pDepth = depthMD.Data();
		}
	}
	if (useScene) 
	{
		mUserGen.GetUserPixels(0, sceneMD);
		nXRes = sceneMD.XRes();
		nYRes = sceneMD.YRes();

		pLabels = sceneMD.Data();
	}
}
예제 #3
0
int main()
{
	XnStatus nRetVal = XN_STATUS_OK;
	Context context;

	nRetVal = context.Init();
	CHECK_RC(nRetVal, "Initialize context");

	DepthGenerator depth;
	nRetVal = depth.Create(context);
	CHECK_RC(nRetVal, "Create depth generator");

	nRetVal = context.StartGeneratingAll();
	CHECK_RC(nRetVal, "StartGeneratingAll");

	DepthMetaData depthMD;
	while (!xnOSWasKeyboardHit())
	{
		nRetVal = context.WaitOneUpdateAll(depth);
		if (nRetVal != XN_STATUS_OK)
		{
			printf("UpdateData failed: %s\n", xnGetStatusString(nRetVal));
			continue;
		}

		depth.GetMetaData(depthMD);
		const XnDepthPixel* pDepthMap = depthMD.Data();

		printf("Frame %d Middle point is: %u.\n", depthMD.FrameID(), depthMD(depthMD.XRes() / 2, depthMD.YRes() / 2));
	}

	context.Shutdown();

	return 0;
}
예제 #4
0
void GeneratePointCloud(DepthGenerator& rDepthGen, const XnDepthPixel* pDepth, VISION_DATA &pData)
{
    DepthMetaData mDepthMD;
    rDepthGen.GetMetaData(mDepthMD);
    pData.timeStamp = mDepthMD.Timestamp();
    unsigned int uPointNum = mDepthMD.FullXRes() * mDepthMD.FullYRes();

    XnPoint3D* pDepthPointSet = new XnPoint3D[uPointNum];
    unsigned int i, j, idxshift, idx;
    for( j = 0; j < mDepthMD.FullYRes(); ++j)
    {
        idxshift = j * mDepthMD.FullXRes();

        for(i = 0; i < mDepthMD.FullXRes(); ++i)
        {
            idx = idxshift + i;
            pDepthPointSet[idx].X = i;
            pDepthPointSet[idx].Y = j;
            pDepthPointSet[idx].Z = pDepth[idx];
        }
    }

    XnPoint3D* p3DPointSet = new XnPoint3D[uPointNum];

    rDepthGen.ConvertProjectiveToRealWorld(uPointNum, pDepthPointSet, p3DPointSet);

    memcpy(pData.pointCloud, p3DPointSet, uPointNum*3*sizeof(float));

    delete[] pDepthPointSet;

    delete[] p3DPointSet;
}
예제 #5
0
// Updates to the latest image obtained from the Kinect
int kinectUpdate(void)
{
  XnStatus nRetVal = context.WaitAndUpdateAll();
  g_image.GetMetaData(g_imageMD);
  //nRetVal = context.WaitOneUpdateAll(depth);
  depth.GetMetaData(depthMD);
  
  return nRetVal;
}
예제 #6
0
int main()
{
    XnStatus nRetVal = XN_STATUS_OK;

    Context context;
    EnumerationErrors errors;

    nRetVal = context.InitFromXmlFile(SAMPLE_XML_PATH, &errors);

    if (nRetVal == XN_STATUS_NO_NODE_PRESENT)
    {
        XnChar strError[1024];
        errors.ToString(strError, 1024);
        printf("%s\n", strError);
        return (nRetVal);
    }
    else if (nRetVal != XN_STATUS_OK)
    {
        printf("Open failed: %s\n", xnGetStatusString(nRetVal));
        return (nRetVal);
    }

    DepthGenerator depth;
    nRetVal = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth);
    CHECK_RC(nRetVal, "Find depth generator");

    XnFPSData xnFPS;
    nRetVal = xnFPSInit(&xnFPS, 180);
    CHECK_RC(nRetVal, "FPS Init");

    DepthMetaData depthMD;

    while (!xnOSWasKeyboardHit())
    {
        nRetVal = context.WaitOneUpdateAll(depth);
        if (nRetVal != XN_STATUS_OK)
        {
            printf("UpdateData failed: %s\n", xnGetStatusString(nRetVal));
            continue;
        }

        xnFPSMarkFrame(&xnFPS);

        depth.GetMetaData(depthMD);
        const XnDepthPixel* pDepthMap = depthMD.Data();

        printf("Frame %d Middle point is: %u. FPS: %f\n", depthMD.FrameID(), depthMD(depthMD.XRes() / 2, depthMD.YRes() / 2), xnFPSCalc(&xnFPS));
    }

    context.Shutdown();

    return 0;
}
예제 #7
0
// Gets the colour and depth data from the Kinect sensor.
bool GetColorAndDepthImages(ColorImage& colorImage, DepthImage& depthImage)
{

	XnStatus rc = XN_STATUS_OK;
	
	// Read a new frame, blocking operation
	rc = deviceContext.WaitAnyUpdateAll();
	if (rc != XN_STATUS_OK)
	{
		/*LOGE("Read failed: %s\n", xnGetStatusString(rc));*/
		throw rc;
	}

	
	// Get handles to new data
	static ImageMetaData colorImageMetaData;
	static DepthMetaData depthImageMetaData;
	colorImageGenerator.GetMetaData(colorImageMetaData);
	depthImageGenerator.GetMetaData(depthImageMetaData);

	
	// Validate images
	if (!depthImageGenerator.IsValid() || !colorImageGenerator.IsValid())
	{
		/*LOGE("Error: Color or depth image is invalid.");*/
		throw 1;
	}

	if (colorImageMetaData.Timestamp() <= mostRecentRGB)
		return false;

	// Fetch pointers to data
	const XnRGB24Pixel* pColorImage = colorImageMetaData.RGB24Data(); //g_depth.GetRGB24ImageMap()
	const XnDepthPixel* pDepthImage = depthImageMetaData.Data();// g_depth.GetDepthMap();
	
	
	// Copy data over to arrays
	memcpy(colorImage.data, pColorImage, sizeof(colorImage.data));
	memcpy(depthImage.data, pDepthImage, sizeof(depthImage.data));
	
	colorImage.rows = colorImage.maxRows;
	colorImage.cols = colorImage.maxCols;

	depthImage.rows = depthImage.maxRows;
	depthImage.cols = depthImage.maxCols;

	mostRecentRGB = colorImageMetaData.Timestamp();
	
	return true;
}
예제 #8
0
void glutDisplay (void){
    glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    // Setup the OpenGL viewpoint
    glMatrixMode(GL_PROJECTION);
    glPushMatrix();
    glLoadIdentity();
    SceneMetaData sceneMD;
    DepthMetaData depthMD;
    ImageMetaData imageMD;
    g_DepthGenerator.GetMetaData(depthMD);
    glOrtho(0, depthMD.XRes(), depthMD.YRes(), 0, -1.0, 1.0);
    glDisable(GL_TEXTURE_2D);
    //XnStatus rc = g_Context.WaitOneUpdateAll(g_DepthGenerator);
    XnStatus rc = g_Context.WaitAnyUpdateAll();
    CHECK_RC("Wait Data",rc);
    g_DepthGenerator.GetMetaData(depthMD);
    if(g_UserGenerator.IsValid())
        g_UserGenerator.GetUserPixels(0, sceneMD);
    g_ImageGenerator.GetMetaData(imageMD);

    DrawDepthMap(depthMD, sceneMD);
    DrawImage(imageMD);
    glutSwapBuffers();
}//glutdisplay
XnStatus prepare(char useScene, char useDepth, char useImage, char useIr, char useHistogram)
{
//TODO handle possible failures! Gotcha!
	if (useDepth)
	{
		mDepthGen.GetMetaData(depthMD);
		nXRes = depthMD.XRes();
		nYRes = depthMD.YRes();

		pDepth = depthMD.Data();

		if (useHistogram)
		{
			calcHist();

			// rewind the pointer
			pDepth = depthMD.Data();
		}
	}
	if (useScene) 
	{
		mUserGen.GetUserPixels(0, sceneMD);
		nXRes = sceneMD.XRes();
		nYRes = sceneMD.YRes();

		pLabels = sceneMD.Data();
	}
	if (useImage)
	{
		mImageGen.GetMetaData(imageMD);
		nXRes = imageMD.XRes();
		nYRes = imageMD.YRes();

		pRGB = imageMD.RGB24Data();
		// HISTOGRAM?????
	}
	if (useIr)
	{
		mIrGen.GetMetaData(irMD);
		nXRes = irMD.XRes();
		nYRes = irMD.YRes();

		pIR = irMD.Data();
		// HISTOGRAM????
	}
}
/*
 * Class:     org_OpenNI_Samples_Assistant_NativeMethods
 * Method:    initFromContext
 * Signature: (JZZ)I
 */
JNIEXPORT jint JNICALL 
Java_org_OpenNI_Samples_Assistant_NativeMethods_initFromContext
  (JNIEnv *env, jclass cls, jlong pContext, jboolean _hasUserGen, jboolean _hasDepthGen)
{
	LOGD("init_start");
	hasUserGen =  _hasUserGen;
	hasDepthGen = _hasDepthGen;

	mContext = new Context((XnContext*) pContext);
	
	if (!(hasUserGen || hasDepthGen))
		return XN_STATUS_BAD_PARAM;
	
	int rc;
	if (hasUserGen)
	{
		rc = mContext->FindExistingNode(XN_NODE_TYPE_USER, mUserGen);
		if (rc != XN_STATUS_OK)
		{
//TODO log&retval
			printf("No user node exists!");
			return 1;
		}

		mUserGen.GetUserPixels(0, sceneMD);
	}

	if (hasDepthGen)
	{
		rc = mContext->FindExistingNode(XN_NODE_TYPE_DEPTH, mDepthGen);
		if (rc != XN_STATUS_OK)
		{
//TODO log&retval
			printf("No depth node exists! Check your XML.");
			return 1;
		}

		mDepthGen.GetMetaData(depthMD);
	}

	initGraphics();

	LOGD("init_end");
	return XN_STATUS_OK;
}
예제 #11
0
void takePhoto() {
    static int index = 1;
    char fname[256] = {0,};
    sprintf(fname, "kinect%03d.txt", index++);

    g_depth.GetMetaData(g_depthMD);
    g_image.GetMetaData(g_imageMD);

    int const nx = g_depthMD.XRes();
    int const ny = g_depthMD.YRes();
    assert(nx == g_imageMD.XRes());
    assert(ny == g_imageMD.YRes());

    const XnDepthPixel* pDepth = g_depthMD.Data();
    const XnUInt8* pImage = g_imageMD.Data();

    FILE * file = fopen(fname, "wb");
    fprintf(file, "%d\n%d\n\n", nx, ny);

    for (int y = 0, di = 0, ri = 0, gi = 1, bi = 2; y < ny; y++) {
        for (int x = 0; x < nx; x++, di++, ri += 3, gi += 3, bi += 3) {
            int const r = pImage[ri];
            int const g = pImage[gi];
            int const b = pImage[bi];
            int const d = pDepth[di];

            assert(r >= 0);
            assert(g >= 0);
            assert(b >= 0);
            assert(d >= 0);

            assert(r <= 0xFF);
            assert(g <= 0xFF);
            assert(b <= 0xFF);
            assert(d <= 0xFFFF);

            fprintf(file, "%3d %3d %3d %5d\n", r, g, b, d);
        }

        fprintf(file, "\n");
    }

    fflush(file);
    fclose(file);
}
예제 #12
0
/*
 *	Function:	run
 *
 *	Starts and continues generating data from the Kinect.
 *	A loop runs and updates data whenever new data is available from one of the Kinect
 *	devices, and then the data is processed to check for patient movement.
 *	The loop is controlled by the "quit" global boolean, which is set to false by the 
 *	signal handler "stop()"
 */
void KinectMonitor::run() {
  XnStatus status;
  SceneMetaData scene;
	DepthMetaData depth;
    
  // Start the device
  status = context.StartGeneratingAll();

  // Running loop
  while( !quit ) {
    // Wait for any new incoming data
    context.WaitOneUpdateAll(depthGenerator);
    // Mark the new frame
		xnFPSMarkFrame(&xnFPS);
    // Get the depth data from the device
		depthGenerator.GetMetaData(depth);
        
    // Get the recognized users
		XnUInt16 numUsers = 15;
		XnUserID users[numUsers];
		userGenerator.GetUsers(users, numUsers);
		// Only track the patient if they are alone
		if( numUsers != 1) continue;
        
		// Get the user data
		userGenerator.GetUserPixels(users[0], scene);
        
    // Update patient position
    previous = current;
    current = getPosition(users[0]);
        
    // Raise alerts based on the patient's state and position
		if( previous != current ) {
        if( current == TURNED && out == false ) {
					// Patient is turned
					printf("Patient getting out of bed.\n");
				} else if( out && bedSet ) {
					printf("Patient is out of bed.\n");
				}
		}
  }
}
예제 #13
0
void readFrame()
{
	if (!g_Depth.IsValid() && !g_Image.IsValid() && !g_IR.IsValid() && !g_Audio.IsValid())	// @@@dded
		return;

	XnStatus rc = XN_STATUS_OK;

	if (g_pPrimary != NULL)
	{
		rc = g_Context.WaitOneUpdateAll(*g_pPrimary);
	}
	else
	{
		rc = g_Context.WaitAnyUpdateAll();
	}

	if (rc != XN_STATUS_OK)
	{
		printf("Error: %s\n", xnGetStatusString(rc));
	}

	if (g_Depth.IsValid())
	{
		g_Depth.GetMetaData(g_DepthMD);
	}

	if (g_Image.IsValid())
	{
		g_Image.GetMetaData(g_ImageMD);
	}

	if (g_IR.IsValid())
	{
		g_IR.GetMetaData(g_irMD);
	}

	if (g_Audio.IsValid())
	{
		g_Audio.GetMetaData(g_AudioMD);
	}
}
예제 #14
0
// Set up OpenNI to obtain 8-bit mono images from the Kinect's RGB camera
int kinectInit(void)
{
  XnStatus nRetVal = XN_STATUS_OK;
  ScriptNode scriptNode;
  EnumerationErrors errors;

  printf("Reading config from: '%s'\n", SAMPLE_XML_PATH_LOCAL);
  nRetVal = context.InitFromXmlFile(SAMPLE_XML_PATH_LOCAL, scriptNode, &errors);

  nRetVal = context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_image); 
  //g_image.SetPixelFormat(XN_PIXEL_FORMAT_GRAYSCALE_8_BIT); 
  g_image.SetPixelFormat(XN_PIXEL_FORMAT_RGB24); 
  g_image.GetMetaData(g_imageMD);

  nRetVal = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth);
  depth.GetMetaData(depthMD);

  //  nRetVal = depth.GetAlternativeViewPointCap().SetViewPoint(g_image);
  //nRetVal = depth.GetFrameSyncCap().FrameSyncWith(g_image);

 return nRetVal;
}
예제 #15
0
//--------------------------------------------------------------
void testApp::setup(){
	
	XnStatus rc;
	
	EnumerationErrors errors;
	rc = g_context.InitFromXmlFile(SAMPLE_XML_PATH, g_scriptNode, &errors);
	if (rc == XN_STATUS_NO_NODE_PRESENT)
	{
		XnChar strError[1024];
		errors.ToString(strError, 1024);
		printf("%s\n", strError);
		return ;
	}
	else if (rc != XN_STATUS_OK)
	{
		printf("Open failed: %s\n", xnGetStatusString(rc));
		return;
	}
	
	rc = g_context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_depth);
	if (rc != XN_STATUS_OK)
	{
		printf("No depth node exists! Check your XML.");
		return;
	}
	
	g_depth.GetMetaData(g_depthMD);
	
	// Texture map init
	g_nTexMapX = (((unsigned short)(g_depthMD.FullXRes()-1) / 512) + 1) * 512;
	g_nTexMapY = (((unsigned short)(g_depthMD.FullYRes()-1) / 512) + 1) * 512;
	g_pTexMap = (XnRGB24Pixel*)malloc(g_nTexMapX * g_nTexMapY * sizeof(XnRGB24Pixel));
	std::cout << " w:" << g_depthMD.FullXRes() << " h:" << g_depthMD.FullYRes() << std::endl;
	pixels = (unsigned char*)malloc(640*480*3*sizeof(unsigned char));
	tex.allocate(640, 480, GL_RGB);
}
예제 #16
0
XnStatus Init_Kinect(EventOutSFNode* skltn,EventOutSFNode* hnz,EventOutSFNode* flr){
	XnStatus rc=XN_STATUS_OK;
	EnumerationErrors errors;
	DepthMetaData g_depthMD;
	ImageMetaData g_imageMD;
	rc = g_context.InitFromXmlFile(SAMPLE_XML_PATH, &errors);
	CHECK_RC(rc, "InitFromXml");
	rc = g_context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_depth);
	CHECK_RC(rc,"XN_NODE_TYPE_DEPTH");
	rc = g_context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_image);
	CHECK_RC(rc,"XN_NODE_TYPE_IMAGE");
	rc=  g_context.FindExistingNode(XN_NODE_TYPE_USER,g_user); 
	CHECK_RC(rc,"XN_NODE_TYPE_USER");
	rc=g_context.FindExistingNode(XN_NODE_TYPE_SCENE,g_scene);
	CHECK_RC(rc,"XN_NODE_TYPE_SCENE");
	rc=g_context.FindExistingNode(XN_NODE_TYPE_HANDS,g_hands);
	CHECK_RC(rc,"XN_NODE_TYPE_HANDS");
	rc=g_context.FindExistingNode(XN_NODE_TYPE_GESTURE,g_gesture);
	CHECK_RC(rc,"XN_NODE_TYPE_GESTURE");
	g_depth.GetMetaData(g_depthMD);
	g_fps=g_depthMD.FPS();
	g_image.GetMetaData(g_imageMD);
	rc=init_skeleton();
	CHECK_RC(rc,"INIT SKELETON");
	rc=init_hands();
	CHECK_RC(rc,"INIT HANDS");
	pix_w=g_depthMD.FullXRes();
	pix_h=g_depthMD.FullYRes();
	if(pix_h==0||pix_w==0){return XN_STATUS_ERROR;}
	g_skltn=skltn;
	g_hnz=hnz;
	g_flr=flr;
	if(NULL==g_skltn||NULL==g_hands||NULL==g_flr)return XN_STATUS_ERROR;
	isInit=true;
	return rc;
}
int main(int argc, char* argv[])
{

	EnumerationErrors errors;


    //rc = context.Init();
    rc = context.InitFromXmlFile(strPathToXML,&errors);
    if (rc == XN_STATUS_NO_NODE_PRESENT)
	{
		XnChar strError[1024];
		errors.ToString(strError, 1024);
		printf("%s\n", strError);
		return (rc);
	}
	else if (rc != XN_STATUS_OK)
	{
		printf("Open failed: %s\n", xnGetStatusString(rc));
		return (rc);
	}
	
	/* UNCOMMENT TO GET FILE READING 
    //rc = context.OpenFileRecording(strInputFile);
	//CHECK_RC(rc, "Open input file");

	//rc = context.FindExistingNode(XN_NODE_TYPE_PLAYER, player);
	//CHECK_RC(rc, "Get player node"); */ 

	rc = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth);
	CHECK_RC(rc, "Find depth generator");

	rc = context.FindExistingNode(XN_NODE_TYPE_IMAGE, image);
	CHECK_RC(rc, "Find image generator");

    depth.GetMetaData(depthMD);
	image.GetMetaData(imageMD);

    //rc = player.SetRepeat(FALSE);
	XN_IS_STATUS_OK(rc);

    //rc = player.GetNumFrames(image.GetName(), nNumFrames);
	//CHECK_RC(rc, "Get player number of frames");
	//printf("%d\n",nNumFrames);

    //rc = player.GetNumFrames(depth.GetName(), nNumFrames);
	//CHECK_RC(rc, "Get player number of frames");
	//printf("%d\n",nNumFrames);

	// Hybrid mode isn't supported
	if (imageMD.FullXRes() != depthMD.FullXRes() || imageMD.FullYRes() != depthMD.FullYRes())
	{
		printf ("The device depth and image resolution must be equal!\n");
		return 1;
	}

	// RGB is the only image format supported.
	if (imageMD.PixelFormat() != XN_PIXEL_FORMAT_RGB24)
	{
		printf("The device image format must be RGB24\n");
		return 1;
	}

    avi = cvCreateVideoWriter(strOutputFile, 0, 30, cvSize(640,480), TRUE);

    depthMetersMat = cvCreateMat(480, 640, CV_16UC1);
    kinectDepthImage = cvCreateImage( cvSize(640,480),16,1 );

    depthMetersMat2 = cvCreateMat(480, 640, CV_16UC1);
    kinectDepthImage2 = cvCreateImage( cvSize(640,480),16,1 );

    colorArr[0] = cv::Mat(imageMD.YRes(),imageMD.XRes(),CV_8U);
    colorArr[1] = cv::Mat(imageMD.YRes(),imageMD.XRes(),CV_8U);
    colorArr[2] = cv::Mat(imageMD.YRes(),imageMD.XRes(),CV_8U);

    //prepare_for_face_detection();

    int b;
    int g;
    int r;

	while ((rc = image.WaitAndUpdateData()) != XN_STATUS_EOF && (rc = depth.WaitAndUpdateData()) != XN_STATUS_EOF) {
        if (rc != XN_STATUS_OK) {
            printf("Read failed: %s\n", xnGetStatusString(rc));
            break;
        }
        depth.GetMetaData(depthMD);
        image.GetMetaData(imageMD);

        //XnUInt32 a;
        //a = g_imageMD.FPS;
        printf("%d\n",imageMD.FrameID());
        //a = g_depthMD.DataSize();
        //printf("%d\n",a);

        pDepth = depthMD.Data();
        pImageRow = imageMD.RGB24Data();

        for (unsigned int y=0; y<imageMD.YRes(); y++) {
            pPixel = pImageRow;
            uchar* Bptr = colorArr[0].ptr<uchar>(y);
            uchar* Gptr = colorArr[1].ptr<uchar>(y);
            uchar* Rptr = colorArr[2].ptr<uchar>(y);

            for(unsigned int x=0;x<imageMD.XRes();++x , ++pPixel){
                Bptr[x] = pPixel->nBlue;
                Gptr[x] = pPixel->nGreen;
                Rptr[x] = pPixel->nRed;

                depthMetersMat->data.s[y * XN_VGA_X_RES + x ] = 7*pDepth[y * XN_VGA_X_RES + x];
                depthMetersMat2->data.s[y * XN_VGA_X_RES + x ] = pDepth[y * XN_VGA_X_RES + x];
            }
            pImageRow += imageMD.XRes();
        }
        cv::merge(colorArr,3,colorImage);
        iplImage = colorImage;

        //cvThreshold(depthMetersMat2, depthMetersMat2, 150, 1500, THRESH_BINARY);

        cvGetImage(depthMetersMat,kinectDepthImage);
        cvGetImage(depthMetersMat2,kinectDepthImage2);

        depthImage = Bw2Image(kinectDepthImage2);
        printf("1. Middle pixel is %u millimeters away\n",depthImage[240][320]);

        rgbImage = RgbImage(&iplImage);

		// we want to see on up to 2000 MM 
        int THRESH = 2000;

        for (unsigned int y=0; y<imageMD.YRes(); y++) {
            for(unsigned int x=0;x<imageMD.XRes();++x){
                if ( depthImage[y][x] >= THRESH ) {
                    depthImage[y][x] = 0;
                } else {
                    float tmp = depthImage[y][x];
                    tmp = tmp / THRESH * (65536)*(-1) + 65536;
                    depthImage[y][x] = (unsigned int)tmp;
                }
            }
        }
		
		// THE PART ABOUT FILTERING COLOURS IN HSV TO SEE ONLY SPECIFIC ONE 
		// AFTER ONE FEW MORPHOLOGICAL OPERATIONS TO MAKE IT LOOK BETTER 

        IplImage* imgHSV = cvCreateImage(cvGetSize(&iplImage), 8, 3);
        cvCvtColor(&iplImage, imgHSV, CV_BGR2HSV);
        imgThreshed = cvCreateImage(cvGetSize(&iplImage), 8, 1);
        //cvInRangeS(imgHSV, cvScalar(100, 60, 80), cvScalar(110, 255, 255), imgThreshed); // BLUE
        cvInRangeS(imgHSV, cvScalar(29, 95, 95), cvScalar(35, 255, 255), imgThreshed); // YELLOW
        //cvInRangeS(imgHSV, cvScalar(29, 60, 60), cvScalar(35, 255, 255), imgThreshed); // YELLOW DARK
        //cvInRangeS(imgHSV, cvScalar(150, 70, 70), cvScalar(160, 255, 255), imgThreshed); // PINK
        //cvInRangeS(imgHSV, cvScalar(40, 76, 76), cvScalar(70, 255, 255), imgThreshed); // GREEN
        IplConvKernel* kernel = cvCreateStructuringElementEx(3, 3, 1, 1, CV_SHAPE_RECT, NULL);
        //cvDilate(imgThreshed,imgThreshed,kernel);
        //cvErode(imgThreshed,imgThreshed,kernel);
        Mat mat = Mat(imgThreshed);
        blur(Mat(imgThreshed),mat,cvSize(3,3));
        imgThreshed = &IplImage(mat);
        //cvInRangeS(imgThreshed,cvScalar(100),cvScalar(255),imgThreshed);
        //cvErode(imgThreshed,imgThreshed,kernel);
        cvDilate(imgThreshed,imgThreshed,kernel);
        cvDilate(imgThreshed,imgThreshed,kernel);
        cvErode(imgThreshed,imgThreshed,kernel);
        cvErode(imgThreshed,imgThreshed,kernel);
        mat = Mat(imgThreshed);
        blur(Mat(imgThreshed),mat,cvSize(6,6));
        imgThreshed = &IplImage(mat);
        cvInRangeS(imgThreshed,cvScalar(100),cvScalar(255),imgThreshed);
        cvReleaseImage(&imgHSV);
        BwImage threshed = BwImage(imgThreshed);



        if ( initialize == true ) {

            normalizeReferenceFace();
            int currentID = 0;

                for ( int y = 30; y<480; y++ ) {
                    for ( int x = 30; x<640; x++ ) {
                        bool g2g = true;
                        //printf("%d %d %d\n",ID, y,x);
                        if ( threshed[y][x]!=0 ) {
                            for ( int ID2 = 0; ID2<nbOfPoints; ID2++) {
                                if ( (abs(markers[ID2].y-y)<proximityLimit) && (abs(markers[ID2].x-x)<proximityLimit)) {
                                    g2g = false;
                                }
                            }
                            if (currentID >= nbOfPoints || g2g == false ) {
                                break;
                            }
                            markers[currentID].y=y;
                            markers[currentID].x=x;
                            currentID++;
                            printf("WHITE PIXEL INITIALIZED %d: %d %d\n",currentID, x,y);
                        }
                    }
                }


            if (isDebugConf==true || currentID == nbOfMarkers) {
                printf("%d PIXELS INITIALIZED\n", currentID);
                initialize = false;
                //printf("%d,%d\n", currentID, nbOfPoints);
                //return 0;
            } else {
                printf("WAITING FOR %d PIXELS TO APPEAR, %d SO FAR \n",nbOfMarkers, currentID);
                continue;
            }


            // FIND TOP RIGHT AND CHIN PIXEL

            int refPixID = 0;
            int chinPixID = 0;

            for ( int i = 0; i < nbOfMarkers; i++) {
                if ( (markers[i].x + markers[i].y)*(markers[i].x + markers[i].y) < (markers[refPixID].x + markers[refPixID].y)* (markers[refPixID].x + markers[refPixID].y)) {
                    refPixID = i;
                }
                if (markers[i].y > markers[chinPixID].y) {
                    chinPixID = i;
                }
            }

            float width = (markers[1].x-markers[0].x)*2;
            float heigth = abs(markers[1].y-markers[0].y);

            // WE GOT WIDTH & HEIGTH OF THE FACE, LETS ADJUST POINTS

            // SET 0 to REF, SET 1 to CHIN

            MyPoint tmp = MyPoint(markers[refPixID].x,markers[refPixID].y);
            markers[refPixID].x = markers[0].x;
            markers[refPixID].y = markers[0].y;
            markers[0].x = tmp.x;
            markers[0].y = tmp.y;

            tmp = MyPoint(markers[chinPixID].x,markers[chinPixID].y);
            markers[chinPixID].x = markers[1].x;
            markers[chinPixID].y = markers[1].y;
            markers[1].x = tmp.x;
            markers[1].y = tmp.y;


            // REST OF THE POINTS

            for ( int i = 2; i < nbOfPoints; i++) {

                int cost = 0;
                int lowestCost = 0;
                int closestPixID = -1;


                for ( int j = 2; j < nbOfMarkers; j++ ) {
                    cost = (markers[j].x-points[i].x*width)*(markers[j].x-points[i].x*width) + (markers[j].y-points[i].y*heigth)*(markers[j].y-points[i].y*heigth);
                    if ( cost < lowestCost ) {
                        lowestCost = cost;
                        closestPixID = j;
                    }
                    if (closestPixID == -1) {
                        //printf("COS JEST SPORO NIE W PORZADKU, CHECK HERE\n");
                        break;
                    }
                    tmp.x = markers[i].x;
                    tmp.y = markers[i].y;
                    markers[i].x=markers[closestPixID].x;
                    markers[i].x=markers[closestPixID].y;
                    markers[closestPixID].x = tmp.x;
                    markers[closestPixID].y = tmp.y;
                }
            }
        }

        for ( int currentPixelID = 0; currentPixelID < nbOfMarkers; currentPixelID++) {
            if (markers[currentPixelID].x == 0) {
                continue;
            }

            if ( threshed[markers[currentPixelID].y][markers[currentPixelID].x] < 128 ) {
                printf("PIXEL %d LOST\n",currentPixelID);

                for ( int neighbSize = 2; neighbSize < maxNeighbSize; neighbSize = neighbSize + 2 ) {

                    int x1 = markers[currentPixelID].x - neighbSize/2;
                    if ( x1 < intoDepthX(0) ) {
                        x1 = (int)intoDepthX(0);
                    }

                    int y1 = (int)(markers[currentPixelID].y-neighbSize/2);
                    if (  y1 < intoDepthY(0) ) {
                        y1 = intoDepthY(0);
                    }

                    int y2 = markers[currentPixelID].y+neighbSize/2;
                    if (  y2 > intoDepthY(480)  ) {
                        y2 = intoDepthY(480);
                    }

                    int x2 = markers[currentPixelID].x+neighbSize/2;
                    if ( x2 > intoDepthX(640) ) {
                        y2 = intoDepthX(640);
                    }

                    bool found = false;
                    for ( int y = y1; y < y2; y++) {
                        for ( int x = x1; x < x2; x++) {
                            bool g2g = true;
                            if (threshed[y][x] > 128) {
                                for ( int ID2 = 0; ID2<nbOfMarkers; ID2++) {
                                    if ( currentPixelID == ID2 )
                                        continue;
                                    if ( (abs(markers[ID2].y-y)<proximityLimit) && (abs(markers[ID2].x-x)<proximityLimit)) {
                                        g2g = false;
                                        break;
                                    }
                                }

                                if ( g2g ) {
                                    markers[currentPixelID].x = x;
                                    markers[currentPixelID].y = y;
                                    found = true;
                                    printf("Pixel %d, FOUND\n",currentPixelID);
                                    break;
                                }
                            }
                        }
                        if (found == true ) {
                            break;
                        }
                    }
                    if (found == true ) {
                        break;
                    }
                }
            }

            paintMarkerOnBoth(markers[currentPixelID]);

        }
        faceImage = cvCreateImage(cvGetSize(&iplImage), 8, 1);
        paintFace();

		// normal kinect depth
        cvShowImage("Depth_Kinect", kinectDepthImage);
		// depth within 80 - 200 mm, normalized 
        cvShowImage("Depth_Kinect_2", kinectDepthImage2);
		// rgb with tracking points
        cvShowImage("RGB_Kinect", &iplImage);
		// colour detector 
        cvShowImage("RGB_Threshed", imgThreshed);
		// attempt to draw a face 
        cvShowImage("Face Image", faceImage);

        cvWaitKey(50);           // wait 20 ms

        if ( avi == NULL) {
            printf ("dupa%d \n",1);
        }
        //cvWriteFrame (avi, &iplImage);
	}

//    cvReleaseImageHeader(kinectDepthImage);
    cvReleaseVideoWriter(&avi);
//    cvReleaseHaarClassifierCascade( &cascade );
    context.Shutdown();

	return 0;
}
예제 #18
0
void glutDisplay (void)
{
	XnStatus rc = XN_STATUS_OK;

	// Read a new frame
	rc = g_context.WaitAnyUpdateAll();
	if (rc != XN_STATUS_OK)
	{
		printf("Read failed: %s\n", xnGetStatusString(rc));
		return;
	}

	g_depth.GetMetaData(g_depthMD);
	g_image.GetMetaData(g_imageMD);

	const XnDepthPixel* pDepth = g_depthMD.Data();
	const XnUInt8* pImage = g_imageMD.Data();

	unsigned int nImageScale = GL_WIN_SIZE_X / g_depthMD.FullXRes();

	// Copied from SimpleViewer
	// Clear the OpenGL buffers
	glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	// Setup the OpenGL viewpoint
	glMatrixMode(GL_PROJECTION);
	glPushMatrix();
	glLoadIdentity();
	glOrtho(0, GL_WIN_SIZE_X, GL_WIN_SIZE_Y, 0, -1.0, 1.0);

	// Calculate the accumulative histogram (the yellow display...)
	xnOSMemSet(g_pDepthHist, 0, MAX_DEPTH*sizeof(float));

	unsigned int nNumberOfPoints = 0;
	for (XnUInt y = 0; y < g_depthMD.YRes(); ++y)
	{
		for (XnUInt x = 0; x < g_depthMD.XRes(); ++x, ++pDepth)
		{
			if (*pDepth != 0)
			{
				g_pDepthHist[*pDepth]++;
				nNumberOfPoints++;
			}
		}
	}
	for (int nIndex=1; nIndex<MAX_DEPTH; nIndex++)
	{
		g_pDepthHist[nIndex] += g_pDepthHist[nIndex-1];
	}
	if (nNumberOfPoints)
	{
		for (int nIndex=1; nIndex<MAX_DEPTH; nIndex++)
		{
			g_pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (g_pDepthHist[nIndex] / nNumberOfPoints)));
		}
	}

	xnOSMemSet(g_pTexMap, 0, g_nTexMapX*g_nTexMapY*sizeof(XnRGB24Pixel));

	// check if we need to draw image frame to texture
	if (g_nViewState == DISPLAY_MODE_OVERLAY ||
		g_nViewState == DISPLAY_MODE_IMAGE)
	{
		const XnRGB24Pixel* pImageRow = g_imageMD.RGB24Data();
		XnRGB24Pixel* pTexRow = g_pTexMap + g_imageMD.YOffset() * g_nTexMapX;

		for (XnUInt y = 0; y < g_imageMD.YRes(); ++y)
		{
			const XnRGB24Pixel* pImage = pImageRow;
			XnRGB24Pixel* pTex = pTexRow + g_imageMD.XOffset();

			for (XnUInt x = 0; x < g_imageMD.XRes(); ++x, ++pImage, ++pTex)
			{
				*pTex = *pImage;
			}

			pImageRow += g_imageMD.XRes();
			pTexRow += g_nTexMapX;
		}
	}

	// check if we need to draw depth frame to texture
	if (g_nViewState == DISPLAY_MODE_OVERLAY ||
		g_nViewState == DISPLAY_MODE_DEPTH)
	{
		const XnDepthPixel* pDepthRow = g_depthMD.Data();
		XnRGB24Pixel* pTexRow = g_pTexMap + g_depthMD.YOffset() * g_nTexMapX;

		for (XnUInt y = 0; y < g_depthMD.YRes(); ++y)
		{
			const XnDepthPixel* pDepth = pDepthRow;
			XnRGB24Pixel* pTex = pTexRow + g_depthMD.XOffset();

			for (XnUInt x = 0; x < g_depthMD.XRes(); ++x, ++pDepth, ++pTex)
			{
				if (*pDepth != 0)
				{
					int nHistValue = g_pDepthHist[*pDepth];
					pTex->nRed = nHistValue;
					pTex->nGreen = nHistValue;
					pTex->nBlue = 0;
				}
			}

			pDepthRow += g_depthMD.XRes();
			pTexRow += g_nTexMapX;
		}
	}

	// Create the OpenGL texture map
	glTexParameteri(GL_TEXTURE_2D, GL_GENERATE_MIPMAP_SGIS, GL_TRUE);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, g_nTexMapX, g_nTexMapY, 0, GL_RGB, GL_UNSIGNED_BYTE, g_pTexMap);

	// Display the OpenGL texture map
	glColor4f(1,1,1,1);

	glBegin(GL_QUADS);

	int nXRes = g_depthMD.FullXRes();
	int nYRes = g_depthMD.FullYRes();

	// upper left
	glTexCoord2f(0, 0);
	glVertex2f(0, 0);
	// upper right
	glTexCoord2f((float)nXRes/(float)g_nTexMapX, 0);
	glVertex2f(GL_WIN_SIZE_X, 0);
	// bottom right
	glTexCoord2f((float)nXRes/(float)g_nTexMapX, (float)nYRes/(float)g_nTexMapY);
	glVertex2f(GL_WIN_SIZE_X, GL_WIN_SIZE_Y);
	// bottom left
	glTexCoord2f(0, (float)nYRes/(float)g_nTexMapY);
	glVertex2f(0, GL_WIN_SIZE_Y);

	glEnd();

	// Swap the OpenGL display buffers
	glutSwapBuffers();
}
예제 #19
0
int main(int argc, char* argv[])
{
	XnStatus nRetVal = XN_STATUS_OK;
	nRetVal = xnLogInitFromXmlFile(SAMPLE_XML_PATH);
	if (nRetVal != XN_STATUS_OK)
	{
		printf("Log couldn't be opened: %s. Running without log", xnGetStatusString(nRetVal));
	}

	if (argc < 3)
	{
		printf("usage: %s <inputFile> <outputFile>\n", argv[0]);
		return -1;
	}

	const char* strInputFile = argv[1];
	const char* strOutputFile = argv[2];

	Context context;
	nRetVal = context.Init();
	CHECK_RC(nRetVal, "Init");

	// open input file
	Player player;
	nRetVal = context.OpenFileRecording(strInputFile, player);
	CHECK_RC(nRetVal, "Open input file");

	// Get depth node from recording
	DepthGenerator depth;
	nRetVal = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth);
	CHECK_RC(nRetVal, "Find depth generator");

	// Create mock node based on depth node from recording
	MockDepthGenerator mockDepth;
	nRetVal = mockDepth.CreateBasedOn(depth);
	CHECK_RC(nRetVal, "Create mock depth node");

	// create recorder
	Recorder recorder;

	nRetVal = recorder.Create(context);
	CHECK_RC(nRetVal, "Create recorder");

	nRetVal = recorder.SetDestination(XN_RECORD_MEDIUM_FILE, strOutputFile);
	CHECK_RC(nRetVal, "Set recorder destination file");

	// add depth node to recorder
	nRetVal = recorder.AddNodeToRecording(mockDepth);
	CHECK_RC(nRetVal, "Add node to recording");

	nRetVal = player.SetRepeat(FALSE);
	XN_IS_STATUS_OK(nRetVal);

	XnUInt32 nNumFrames = 0;

	nRetVal = player.GetNumFrames(depth.GetName(), nNumFrames);
	CHECK_RC(nRetVal, "Get player number of frames");

	DepthMetaData depthMD;

	while ((nRetVal = depth.WaitAndUpdateData()) != XN_STATUS_EOF)
	{
		CHECK_RC(nRetVal, "Read next frame");
		
		// Get depth meta data
		depth.GetMetaData(depthMD);

		//-----------------------------------------------//
		// Transform depth! This is the interesting part //
		//-----------------------------------------------//
		
		/* Enable the depth data to be modified. This is done implicitly by depthMD.WritableDepthMap(),
		   but we're calling it just to be clear. */
		nRetVal = depthMD.MakeDataWritable();
		CHECK_RC(nRetVal, "Make depth data writable");

		transformDepthMD(depthMD);

		// Pass the transformed data to the mock depth generator
		nRetVal = mockDepth.SetData(depthMD);
		CHECK_RC(nRetVal, "Set mock node new data");

		/* We need to call recorder.Record explicitly because we're not using WaitAndUpdateAll(). */
		nRetVal = recorder.Record();
		CHECK_RC(nRetVal, "Record");

		printf("Recorded: frame %u out of %u\r", depthMD.FrameID(), nNumFrames);
	}

	printf("\n");

	return 0;
}
예제 #20
0
void captureDepthMap(unsigned char* g_ucDepthBuffer)
{
	SceneMetaData smd;
	DepthMetaData dmd;
	_depth.GetMetaData(dmd);

	//printf("AS3OpenNI :: Frame %d Middle point is: %u. FPS: %f\n", dmd.FrameID(), dmd(dmd.XRes() / 2, dmd.YRes() / 2), xnFPSCalc(&xnFPS));

	_depth.GetMetaData(dmd);
	_userGenerator.GetUserPixels(0, smd);
	unsigned int nValue = 0;
	unsigned int nHistValue = 0;
	unsigned int nIndex = 0;
	unsigned int nX = 0;
	unsigned int nY = 0;
	unsigned int nNumberOfPoints = 0;
	XnUInt16 g_nXRes = dmd.XRes();
	XnUInt16 g_nYRes = dmd.YRes();

	const XnDepthPixel* pDepth = dmd.Data();
	const XnLabel* pLabels = smd.Data();

	// Calculate the accumulative histogram
	memset(g_pDepthHist, 0, MAX_DEPTH*sizeof(float));
	for (nY=0; nY<g_nYRes; nY++)
	{
		for (nX=0; nX<g_nXRes; nX++)
		{
			nValue = *pDepth;
			if (nValue != 0)
			{
				g_pDepthHist[nValue]++;
				nNumberOfPoints++;
			}
			pDepth++;
		}
	}

	for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
	{
		g_pDepthHist[nIndex] += g_pDepthHist[nIndex-1];
	}

	if (nNumberOfPoints)
	{
		for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
		{
			g_pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (g_pDepthHist[nIndex] / nNumberOfPoints)));
		}
	}
	
	pDepth = dmd.Data();
	if (_drawPixels)
	{
		XnUInt32 nIndex = 0;
		for (nY=0; nY<g_nYRes; nY++)
		{
			for (nX=0; nX < g_nXRes; nX++, nIndex++)
			{
				g_ucDepthBuffer[0] = 0;
				g_ucDepthBuffer[1] = 0;
				g_ucDepthBuffer[2] = 0;
				g_ucDepthBuffer[3] = 0x00;
				if (_depthMapBackground || *pLabels != 0)
				{
					nValue = *pDepth;
					XnLabel label = *pLabels;
					XnUInt32 nColorID = label % nColors;
					if (label == 0)
					{
						nColorID = nColors;
					}

					if (nValue != 0)
					{
						nHistValue = g_pDepthHist[nValue];
						if(_depthMapDetect)
						{
							g_ucDepthBuffer[0] = nHistValue * Colors[nColorID][0]; 
							g_ucDepthBuffer[1] = nHistValue * Colors[nColorID][1];
							g_ucDepthBuffer[2] = nHistValue * Colors[nColorID][2];
						}
						else
						{
							g_ucDepthBuffer[0] = nHistValue; 
							g_ucDepthBuffer[1] = nHistValue;
							g_ucDepthBuffer[2] = nHistValue;
						}
						
						g_ucDepthBuffer[3] = 0xFF;
					}
				}
				pDepth++;
				pLabels++;
				g_ucDepthBuffer+=4;
			}
		}
	}
}
예제 #21
0
int main(int argc, char* argv[])
{
	XnStatus rc;

	EnumerationErrors errors;
	rc = g_context.InitFromXmlFile(SAMPLE_XML_PATH, &errors);
	if (rc == XN_STATUS_NO_NODE_PRESENT)
	{
		XnChar strError[1024];
		errors.ToString(strError, 1024);
		printf("%s\n", strError);
		return (rc);
	}
	else if (rc != XN_STATUS_OK)
	{
		printf("Open failed: %s\n", xnGetStatusString(rc));
		return (rc);
	}

	rc = g_context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_depth);
	rc = g_context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_image);

	g_depth.GetMetaData(g_depthMD);
	g_image.GetMetaData(g_imageMD);

	// Hybrid mode isn't supported in this sample
	if (g_imageMD.FullXRes() != g_depthMD.FullXRes() || g_imageMD.FullYRes() != g_depthMD.FullYRes())
	{
		printf ("The device depth and image resolution must be equal!\n");
		return 1;
	}

	// RGB is the only image format supported.
	if (g_imageMD.PixelFormat() != XN_PIXEL_FORMAT_RGB24)
	{
		printf("The device image format must be RGB24\n");
		return 1;
	}

	// Texture map init
	g_nTexMapX = (((unsigned short)(g_depthMD.FullXRes()-1) / 512) + 1) * 512;
	g_nTexMapY = (((unsigned short)(g_depthMD.FullYRes()-1) / 512) + 1) * 512;
	g_pTexMap = (XnRGB24Pixel*)malloc(g_nTexMapX * g_nTexMapY * sizeof(XnRGB24Pixel));

	// OpenGL init
	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
	glutInitWindowSize(GL_WIN_SIZE_X, GL_WIN_SIZE_Y);
	glutCreateWindow ("OpenNI Simple Viewer");
	glutFullScreen();
	glutSetCursor(GLUT_CURSOR_NONE);

	glutKeyboardFunc(glutKeyboard);
	glutDisplayFunc(glutDisplay);
	glutIdleFunc(glutIdle);

	glDisable(GL_DEPTH_TEST);
	glEnable(GL_TEXTURE_2D);

	// Per frame code is in glutDisplay
	glutMainLoop();

	return 0;
}
예제 #22
0
//----------------------------------------------------
// 描画処理
//----------------------------------------------------
void glutDisplay (void){
	xnFPSMarkFrame(&g_xnFPS);		// FPSの計測開始?

	XnStatus rc = XN_STATUS_OK;

	// 更新されたノードを待つ(どれでもいい)
	rc = g_context.WaitAnyUpdateAll();
	if (rc != XN_STATUS_OK){
		printf("Read failed: %s\n", xnGetStatusString(rc));
		printf("test\n");
		return;
	}

	// イメージ・デプス・ユーザのデータを取得
	g_image.GetMetaData(g_imageMD);
	g_depth.GetMetaData(g_depthMD);
	g_user.GetUserPixels(0, g_sceneMD);

	// カラー・デプスバッファをクリア
	glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	// 設定
	setDepthHistgram(g_depth, g_depthMD, g_pDepthHist);	// ヒストグラムの計算・作成
	setTexture();	// テクスチャ設定

	// 描画
	drawImage();	// イメージデータの描画

	// デバッグモードの文字は描画の行列と隔離
	glMatrixMode(GL_PROJECTION);								// 射影変換の行列の設定
	//glLoadIdentity();											// スタックのクリア
	glMatrixMode(GL_MODELVIEW);								// モデルビュー変換の行列の設定
	glLoadIdentity();
	if(g_debugMode) glDebug();								// デバッグモード

	// 一度だけスクリーンショットをとる
	if(g_screenShotImageMode){
		ostringstream fname;
		fname	<< OUT_IMAGE_PATH ;//出力ファイル名
		std::string name = fname.str();
		g_glScreenShot.screenshot(name.c_str(), 24);

		g_screenShotImageMode = !g_screenShotImageMode;	// トグル
	}

	// 一度だけ深さデータを取得する
	if(g_screenShotDepthMode){
		ofstream ofs(OUT_DEPTH_PATH);
		const XnDepthPixel* pDepth = g_depthMD.Data();

		for (XnUInt y = 0; y < KINECT_IMAGE_HEIGHT; y ++){
			for (XnUInt x = 0; x < KINECT_IMAGE_WIDTH; x ++, pDepth ++){
				if(*pDepth < 2000){
					ofs << (int)((*pDepth) * 2) << ',';
				}else{
					ofs << (*pDepth) << ',';
				}
			}
			ofs << endl;
		}

		g_screenShotDepthMode = !g_screenShotDepthMode;	// トグル
	}

	// Swap the OpenGL display buffers
	glutSwapBuffers();
}
int main(int argc, char* argv[])
{
    XnStatus nRetVal = XN_STATUS_OK;
    nRetVal = xnLogInitFromXmlFile(SAMPLE_XML_PATH);
    if (nRetVal != XN_STATUS_OK)
    {
        printf("Log couldn't be opened: %s. Running without log", xnGetStatusString(nRetVal));
    }
    if (argc < 3)
    {
        printf("usage: %s <inputFile> <outputFile>\n", argv[0]);
        return -1;
    }
    const char* strInputFile = argv[1];
    const char* strOutputFile = argv[2];
    Context context;
    nRetVal = context.Init();
    CHECK_RC(nRetVal, "Init");
    // open input file
    Player player;
    nRetVal = context.OpenFileRecording("/media/6B58CB581C0AACF6/7.oni", player);
    CHECK_RC(nRetVal, "Open input file");
    // Get depth node from recording
    DepthGenerator depth;
    nRetVal = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth);
    CHECK_RC(nRetVal, "Find depth generator");
    // Create mock node based on depth node from recording
    MockDepthGenerator mockDepth;
    nRetVal = mockDepth.CreateBasedOn(depth);
    CHECK_RC(nRetVal, "Create mock depth node");


    ImageGenerator image;
    nRetVal = context.FindExistingNode(XN_NODE_TYPE_IMAGE, image);
    CHECK_RC(nRetVal, "Find depth generator");
    // Create mock node based on depth node from recording
    MockImageGenerator mockImage;
    nRetVal = mockImage.CreateBasedOn(image);
    CHECK_RC(nRetVal, "Create mock depth node");
    // create recorder
    Recorder recorder;
    nRetVal = recorder.Create(context);
    CHECK_RC(nRetVal, "Create recorder");
    nRetVal = recorder.SetDestination(XN_RECORD_MEDIUM_FILE, "/home/shaghayegh/up.oni");
    CHECK_RC(nRetVal, "Set recorder destination file");
    // add depth node to recorder
    nRetVal = recorder.AddNodeToRecording(mockDepth);
    CHECK_RC(nRetVal, "Add node to recording");
    // nRetVal = recorder.AddNodeToRecording(mockImage);
    // CHECK_RC(nRetVal, "Add node to recording");

    nRetVal = player.SetRepeat(FALSE);
    XN_IS_STATUS_OK(nRetVal);
    XnUInt32 nNumFrames = 0;
    nRetVal = player.GetNumFrames(depth.GetName(), nNumFrames);
    CHECK_RC(nRetVal, "Get player number of frames");
    DepthMetaData depthMD;
    ImageMetaData imageMD;
    int frameNum = 0;
    String path = "/media/6B58CB581C0AACF6/ebook/Articles/activity_recognition/data1/0512164529/";
    while ((nRetVal = depth.WaitAndUpdateData()) != XN_STATUS_EOF)
    {
        ++frameNum;
        CHECK_RC(nRetVal, "Read next frame");
        // Get depth meta data
        depth.GetMetaData(depthMD);
        image.GetMetaData(imageMD);

        //-----------------------------------------------//
        // Transform depth! This is the interesting part //
        //-----------------------------------------------//
        /* Enable the depth data to be modified. This is done implicitly by depthMD.WritableDepthMap(),
but we're calling it just to be clear. */
        nRetVal = depthMD.MakeDataWritable();
        CHECK_RC(nRetVal, "Make depth data writable");

        // nRetVal = imageMD.MakeDataWritable();
        // CHECK_RC(nRetVal, "Make depth data writable");

        String ficheroActualRGB;
        // ficheroActualRGB = path  +"RGB_" + boost::to_string(frameNum) + ".png";
        String ficheroActualDepth = path +"Depth_"+ boost::to_string(frameNum) + ".png";

        // Mat matFrameImage = imread(ficheroActualRGB, 1);
        // resize(matFrameImage, matFrameImage, Size(640, 480), 0, 0, INTER_CUBIC);
        Mat matFrameDepth = imread(ficheroActualDepth,1);
        resize(matFrameDepth, matFrameDepth, Size(480, 640), 0, 0, INTER_CUBIC);

        transformDepthMD(matFrameDepth,depthMD);
        // transformImageMD(matFrameImage,imageMD);
//         Pass the transformed data to the mock depth generator
        nRetVal = mockDepth.SetData(depthMD);
        CHECK_RC(nRetVal, "Set mock node new data");

        // nRetVal = mockImage.SetData(imageMD);
        // CHECK_RC(nRetVal, "Set mock node new data");

        /* We need to call recorder.Record explicitly because we're not using WaitAndUpdateAll(). */
        nRetVal = recorder.Record();
        CHECK_RC(nRetVal, "Record");
        printf("Recorded: frame %u out of %u\r", depthMD.FrameID(), nNumFrames);
    }
    printf("\n");
    return 0;
}
예제 #24
0
//----------------------------------------------------
// OpenNI関連の初期化
//----------------------------------------------------
void xnInit(void){
	XnStatus rc;

	EnumerationErrors errors;
	rc = g_context.InitFromXmlFile(SAMPLE_XML_PATH, &errors);
	if (rc == XN_STATUS_NO_NODE_PRESENT){
		XnChar strError[1024];
		errors.ToString(strError, 1024);
		printf("%s\n", strError);
		exit(1);
	}else if (rc != XN_STATUS_OK){
		printf("Open failed: %s\n", xnGetStatusString(rc));
		exit(1);
	}
	
	//playerInit();

	rc = xnFPSInit(&g_xnFPS, 180);	// FPSの初期化
	//CHECK_RC(rc, "FPS Init");

	// デプス・イメージ・ユーザジェネレータの作成
	rc = g_context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_depth);
	errorCheck(rc, "g_depth");		// エラーチェック
	rc = g_context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_image);
	errorCheck(rc, "g_image");
	rc = g_context.FindExistingNode(XN_NODE_TYPE_USER, g_user);
	//rc = g_user.Create(g_context);
	errorCheck(rc, "g_user");

	// ユーザー検出機能をサポートしているか確認
	if (!g_user.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) {
		//throw std::runtime_error("ユーザー検出をサポートしてません");
		cout << "ユーザー検出をサポートしてません" << endl;
		exit(1);
	}

	// レコーダーの設定
	//rc = setRecorder(g_recorder, rc);

	// ユーザコールバックの登録
	XnCallbackHandle userCallbacks;
	g_user.RegisterUserCallbacks(UserDetected, UserLost, NULL, userCallbacks);

	// デプス・イメージ・ユーザデータの取得
	g_depth.GetMetaData(g_depthMD);
	g_image.GetMetaData(g_imageMD);
	g_user.GetUserPixels(0, g_sceneMD);

	// Hybrid mode isn't supported in this sample
	// イメージとデプスの大きさが違うとエラー
	if (g_imageMD.FullXRes() != g_depthMD.FullXRes() || g_imageMD.FullYRes() != g_depthMD.FullYRes()){
		printf ("The device depth and image resolution must be equal!\n");
		exit(1);
	}

	// RGB is the only image format supported.
	// フォーマットの確認
	if (g_imageMD.PixelFormat() != XN_PIXEL_FORMAT_RGB24){
		printf("The device image format must be RGB24\n");
		exit(1);
	}

	// Texture map init
	// フルスクリーン画面の大きさ調整
	g_nTexMapX = (((unsigned short)(g_depthMD.FullXRes() - 1) / 512) + 1) * 512;	// 大きさによって512の倍数に調整(1024)
	g_nTexMapY = (((unsigned short)(g_depthMD.FullYRes() - 1) / 512) + 1) * 512;	// 512
	g_pTexMap = (XnRGB24Pixel*)malloc(g_nTexMapX * g_nTexMapY * sizeof(XnRGB24Pixel));	// スクリーンの大きさ分の色情報の容量を確保

	// 座標ポインタの初期化
	g_pPoint = (XnPoint3D*)malloc(KINECT_IMAGE_SIZE * sizeof(XnPoint3D));			// 座標を入れるポインタを作成
	g_pBackTex = (XnRGB24Pixel*)malloc(KINECT_IMAGE_SIZE * sizeof(XnRGB24Pixel));	// 背景画像を入れるポインタを作成
	g_pBackPoint = (XnPoint3D*)malloc(KINECT_IMAGE_SIZE * sizeof(XnPoint3D));		// 背景座標を入れるポインタを作成
	g_pBackDepth = (XnDepthPixel*)malloc(KINECT_IMAGE_SIZE * sizeof(XnDepthPixel));		// 背景座標を入れるポインタを作成
}
예제 #25
0
int _tmain(int argc, _TCHAR* argv[])
{
	XnStatus nRetVal = XN_STATUS_OK;
	Context context;

	nRetVal = context.Init();
	DepthGenerator depth;
	nRetVal = depth.Create(context);

	XnMapOutputMode mapMode;

	mapMode.nXRes = XN_VGA_X_RES;
	mapMode.nYRes = XN_VGA_Y_RES; 
	mapMode.nFPS = 30; 

	nRetVal = depth.SetMapOutputMode(mapMode);

	nRetVal = context.StartGeneratingAll();

	XnUInt32 nMiddleIndex = XN_VGA_X_RES * XN_VGA_Y_RES/2 + XN_VGA_X_RES/2;

	int count = 0;
	const XnDepthPixel* pDepthMap;

	while (count<50) { // Update to next frame 
		nRetVal = context.WaitOneUpdateAll(depth);
		pDepthMap = depth.GetDepthMap(); 
		printf("Middle pixel is %u millimeters away\n", pDepthMap[nMiddleIndex]);
		count++;
	} 

	DepthMetaData g_depthMD;
	depth.GetMetaData(g_depthMD);

	cout<<g_depthMD.FullXRes();
	cout<<g_depthMD.FullYRes();

	
	const double maxDepth = 3000;

	const int xScale = 1;
	const int yScale = 1;

	const int xActualRes = XN_VGA_X_RES;
	const int yActualRes = XN_VGA_Y_RES;

	const int xRes = xActualRes/xScale;
	const int yRes = yActualRes/yScale;

	const int sizeOfMap = xRes * yRes + 2*xRes + 2*(yRes-2) +2 ; //multiply by two for back face

	vertex* vertices = new vertex[sizeOfMap];

	double xRealScale = 1.0;
	double yRealScale = 1.0;

	for (int j = 0; j < yRes; j++) {
		for (int i = 0; i < xRes; i++) {
		
			double x = xScale * xRealScale * i;
			double y = yScale * yRealScale * j;

			double z = -(double) g_depthMD.DepthMap()[xActualRes * j * yScale + i * xScale];

			if (z == 0) 
				z = -maxDepth;

			if (z > 1800) 
				z = -maxDepth;

			vertices[xRes*j + i] = vertex(x,y,z);
		}
	}

	int offset = xRes*yRes;

	vector<int> indices;

	//bottom facets
	for (int i = 0; i< xRes; i++) {
		double x = xRealScale * xScale* i;
		double y = 0;
		double z = -maxDepth - 100;  //placeholder

		vertices[offset + i] = vertex(x,y,z);
	}

	//bottom facet winding
	for (int i = 0; i< xRes-1; i++) {
		indices.push_back(i);				
		indices.push_back(i+1);				
		indices.push_back(xRes*yRes + 1 + i);	

		indices.push_back(i);				
		indices.push_back(xRes*yRes + 1 + i);	
		indices.push_back(xRes*yRes + i);	
	}

	offset = offset + xRes;

	//right side facets 
	for (int i = 1; i< yRes; i++) {
		double x = xRealScale * xScale * (xRes-1);
		double y = yRealScale * yScale * i;
		double z = -maxDepth - 100;  //placeholder

		vertices[offset + i -1] = vertex(x,y,z);
	}

	offset = offset + yRes -1;

	//right facet winding
	for (int i = 0; i< yRes-1; i++) {

		indices.push_back(xRes-1 + i * xRes);			//2	 good		
		indices.push_back(xRes-1 + xRes * (i+1));		//5	 			
		indices.push_back(xRes*yRes + xRes + i);		//12

		indices.push_back(xRes-1 + i * xRes);			//2	 good		
		indices.push_back(xRes*yRes + xRes + i -1);		//11	
		indices.push_back(xRes*yRes + xRes + i);		//12
	}


	//top side facets 
	for (int i = 1; i< xRes; i++) {
		double x = xRealScale * xScale * (xRes-1 - i);
		double y = yRealScale * yScale * (yRes-1);
		double z = -maxDepth - 100;  //placeholder

		vertices[offset + i -1] = vertex(x,y,z);
	}

	//top facet winding
	for (int i = 0; i< xRes-1; i++) {

		indices.push_back(xRes*yRes-1 - i);					//8	 good		
		indices.push_back(xRes*yRes -1 + xRes + yRes + i);	//14	 			
		indices.push_back(xRes*yRes -2 - i);				//7

		indices.push_back(xRes*yRes-1 - i);					//8	 good	
		indices.push_back(xRes*yRes -1 + xRes + yRes + i -1);	//13	
		indices.push_back(xRes*yRes -1 + xRes + yRes + i);		//14
	}

	offset = offset + xRes - 1; //check

	//left side facet 
	for (int i = 1; i< yRes+1; i++) {
		double x = 0;
		double y = yRealScale * yScale * (yRes - i); //check
		double z = -maxDepth - 100;  //placeholder

		vertices[offset + i -1] = vertex(x,y,z); //check
	}

	//left side winding
	for (int i = 0; i < yRes-1; i++) {
		indices.push_back(xRes*yRes - (xRes*(i+2)));	 //3
		indices.push_back(xRes*yRes - (xRes*(i+1)));	//6
		indices.push_back(xRes*yRes + 2*xRes + yRes -3 +i);  //15

		indices.push_back(xRes*yRes + 2*xRes + yRes -3 + i + 1);  //16
		indices.push_back(xRes*yRes - (xRes*(i+2)));
		indices.push_back(xRes*yRes + 2*xRes + yRes -3 +i); //15

		//indices.push_back(xRes*yRes - xRes * (i+1));			//6	
		//indices.push_back(xRes*yRes + 2*xRes + yRes - 3 + i);	//15
		//indices.push_back(xRes*yRes - xRes * (i+2));	 		//3	

		//indices.push_back(xRes*yRes - xRes * (i+2));	 			//3	
		//indices.push_back(xRes*yRes + 2*xRes + yRes - 3 + i);		//15
		//indices.push_back(xRes*yRes + 2*xRes + yRes - 3 + i +1);	//16

	}

	
		indices.push_back(xRes*yRes);			//6	
		indices.push_back(0);			//15
		indices.push_back(xRes*yRes + 2*xRes + 2*yRes -4);	

	//bottom face
		indices.push_back(xRes*yRes);			//6	
		indices.push_back(xRes*yRes + xRes-1);			//15
		indices.push_back(xRes*yRes + xRes + yRes -2);	 		//3	

		indices.push_back(xRes*yRes);			//6	
		indices.push_back(xRes*yRes + xRes + yRes -2);			//15
		indices.push_back(xRes*yRes + 2*xRes + yRes -3);		//3	


	printf("Created %u vertices\n",sizeOfMap);

	//front face - this shows the actual depth map
	for (int i = 0; i < xRes-1; i++) {
		for (int j = 0; j < yRes-1; j++) {
			indices.push_back(j * xRes + i);
			indices.push_back((j+1) * xRes + i);
			indices.push_back(j * xRes + i + 1);
			
			indices.push_back(j * xRes + i + 1);
			indices.push_back((j+1) * xRes + i);
			indices.push_back((j+1) * xRes + i + 1);
		}
	}

	int numFacets = indices.size()/3;

	printf("Created %u facets\n",numFacets);

	//int offset = sizeOfMap-1; // number of vertices on front face

	////back face
	////for (int i = 0; i < xRes-1; i++) {
	////	for (int j = 0; j < yRes-1; j++) {
	////		indices.push_back(j * xRes + i + 1 + offset);
	////		indices.push_back((j+1) * xRes + i + offset);
	////		indices.push_back(j * xRes + i + offset);
	////		
	////		indices.push_back((j+1) * xRes + i + 1 + offset);
	////		indices.push_back((j+1) * xRes + i + offset);
	////		indices.push_back(j * xRes + i + 1 + offset);
	////	}
	////}

	//int n = indices.size()/3;

	ofstream stlOut;
	stlOut.open ("stlOut.stl");
	printf("Writing to STL...\n");
	stlOut << "solid kinectout\n";

	//  printf("Face Count: %u Vertex Count: %u", numFacets, xRes*yRes);

    for (int ii = 0; ii < numFacets; ++ii)
    {

        int v1 = indices[3*ii];
        int v2 = indices[3*ii + 1];
        int v3 = indices[3*ii + 2];

		vertex p1 = vertices[v1];
        vertex p2 = vertices[v2];
        vertex p3 = vertices[v3];
 
        vertex dir1 = p1.subtract(p2);
        vertex dir2  = p3.subtract(p2);

        vertex n = dir1.cross(dir2).normalize();
 
		stlOut << "  facet normal "<< n.x << " " << n.y << " " << n.z << "\n";
		stlOut << "    outer loop\n";
		stlOut << "      vertex " << p1.x << " " << p1.y << " " << p1.z << "\n";
		stlOut << "      vertex " << p2.x << " " << p2.y << " " << p2.z << "\n";
		stlOut << "      vertex " << p3.x << " " << p3.y << " " << p3.z << "\n";
		stlOut << "    endloop\n";
		stlOut << "  endfacet\n";

    }
	stlOut << "endsolid kinectout";

	printf("Complete!\n");
	stlOut.close();

}
예제 #26
0
int main(int argc, char *argv[])
{	
	//--------------------------------------------------------------------//
	//------------------------- SETUP REQUIRED NODES ---------------------//
	//--------------------------------------------------------------------//
	
	// Setup the command line parameters.
	setupParams(argc, argv);
	
	// Setup all the sockets.
	setupSockets();
    
	// Setup the capture socket server for Mac.
	#if (XN_PLATFORM == XN_PLATFORM_MACOSX)
		if(_featureDepthMapCapture || _featureRGBCapture)
		{
			if(_useSockets)
			{
				g_AS3Network = network();
				g_AS3Network.init(setupServer);
			}
		}
	#endif
	
	// Setup the status.
    XnStatus _status = XN_STATUS_OK;
    EnumerationErrors _errors;
    
    // Context Init and Add license.
	_status = _context.Init();
	CHECK_RC(_status, "AS3OpenNI :: Initialize context");
	_context.SetGlobalMirror(_mirror);
	
	XnChar vendor[XN_MAX_NAME_LENGTH];
	XnChar license[XN_MAX_LICENSE_LENGTH];

	_license.strVendor[XN_MAX_NAME_LENGTH] = strcmp(vendor, "PrimeSense");
	_license.strKey[XN_MAX_LICENSE_LENGTH] = strcmp(license, "0KOIk2JeIBYClPWVnMoRKn5cdY4=");
		
	_status = _context.AddLicense(_license);
   	CHECK_RC(_status, "AS3OpenNI :: Added license");
   	
   	// Set it to VGA maps at 30 FPS
	_depthMode.nXRes = 640;
	_depthMode.nYRes = 480;
	_depthMode.nFPS = 30;
	
	// Depth map create.
	_status = _depth.Create(_context);
	CHECK_RC(_status, "AS3OpenNI :: Create depth generator");
	_status = _depth.SetMapOutputMode(_depthMode);
	
	// Depth map create.
	_status = _image.Create(_context);
	CHECK_RC(_status, "AS3OpenNI :: Create image generator");
	_status = _image.SetMapOutputMode(_depthMode);
	_status = _image.SetPixelFormat(XN_PIXEL_FORMAT_RGB24);
	
	// Create the hands generator.
	_status = _hands.Create(_context);
	CHECK_RC(_status, "AS3OpenNI :: Create hands generator");
	_hands.SetSmoothing(0.1);

	// Create the gesture generator.
	_status = _gesture.Create(_context);
	CHECK_RC(_status, "AS3OpenNI :: Create gesture generator");
	
	// Create user generator.
	_status = _userGenerator.Create(_context);
	CHECK_RC(_status, "AS3OpenNI :: Find user generator");
	
	// Create and initialize point tracker
	_sessionManager = new XnVSessionManager();
	_status = _sessionManager->Initialize(&_context, "Wave", "RaiseHand");
	
	if (_status != XN_STATUS_OK)
	{
		printf("AS3OpenNI :: Couldn't initialize the Session Manager: %s\n", xnGetStatusString(_status));
		CleanupExit();
	}
	_sessionManager->RegisterSession(NULL, &SessionStart, &SessionEnd, &SessionProgress);
	
	// Start catching signals for quit indications
	CatchSignals(&_quit);
	
	//---------------------------------------------------------------//
	//------------------------- SETUP FEATURES ---------------------//
	//--------------------------------------------------------------//
	
	// Define the Wave and SinglePoint detectors.
	_waveDetector = new XnVWaveDetector();
	
	// SinglePoint detector.
	if(_featureSinglePoint) _waveDetector->RegisterPointUpdate(NULL, &OnPointUpdate);
	
	// Feature Gesture.
	if(_featureGesture)
	{
		// Wave detector.
		_waveDetector->RegisterWave(NULL, &OnWave);
		
		// Push detector.
		_pushDetector = new XnVPushDetector();
		_pushDetector->RegisterPush(NULL, &onPush);
	
		// Swipe detector.
		_swipeDetector = new XnVSwipeDetector();
		_swipeDetector->RegisterSwipeUp(NULL, &Swipe_SwipeUp);
		_swipeDetector->RegisterSwipeDown(NULL, &Swipe_SwipeDown);
		_swipeDetector->RegisterSwipeLeft(NULL, &Swipe_SwipeLeft);
		_swipeDetector->RegisterSwipeRight(NULL, &Swipe_SwipeRight);
	
		// Steady detector.
		_steadyDetector = new XnVSteadyDetector();
		_steadyDetector->RegisterSteady(NULL, &Steady_OnSteady);
	}
	
	// Feature Circle.
	if(_featureCircle)
	{
		// Circle detector.
		_circleDetector = new XnVCircleDetector();
		_circleDetector->RegisterCircle(NULL, &CircleCB);
		_circleDetector->RegisterNoCircle(NULL, &NoCircleCB);
		_circleDetector->RegisterPrimaryPointCreate(NULL, &Circle_PrimaryCreate);
		_circleDetector->RegisterPrimaryPointDestroy(NULL, &Circle_PrimaryDestroy);
	}
	
	// Feature Slider.
	if(_featureSlider)
	{
		// Left/Right slider.
		_leftRightSlider = new XnVSelectableSlider1D(3, 0, AXIS_X);
		_leftRightSlider->RegisterActivate(NULL, &LeftRightSlider_OnActivate);
		_leftRightSlider->RegisterDeactivate(NULL, &LeftRightSlider_OnDeactivate);
		_leftRightSlider->RegisterPrimaryPointCreate(NULL, &LeftRightSlider_OnPrimaryCreate);
		_leftRightSlider->RegisterPrimaryPointDestroy(NULL, &LeftRightSlider_OnPrimaryDestroy);
		_leftRightSlider->RegisterValueChange(NULL, &LeftRightSlider_OnValueChange);
		_leftRightSlider->SetValueChangeOnOffAxis(false);
		
		// Up/Down slider.
		_upDownSlider = new XnVSelectableSlider1D(3, 0, AXIS_Y);
		_upDownSlider->RegisterActivate(NULL, &UpDownSlider_OnActivate);
		_upDownSlider->RegisterDeactivate(NULL, &UpDownSlider_OnDeactivate);
		_upDownSlider->RegisterPrimaryPointCreate(NULL, &UpDownSlider_OnPrimaryCreate);
		_upDownSlider->RegisterPrimaryPointDestroy(NULL, &UpDownSlider_OnPrimaryDestroy);
		_upDownSlider->RegisterValueChange(NULL, &UpDownSlider_OnValueChange);
		_upDownSlider->SetValueChangeOnOffAxis(false);
		
		// In/Out slider.
		_inOutSlider = new XnVSelectableSlider1D(3, 0, AXIS_Z);
		_inOutSlider->RegisterActivate(NULL, &InOutSlider_OnActivate);
		_inOutSlider->RegisterDeactivate(NULL, &InOutSlider_OnDeactivate);
		_inOutSlider->RegisterPrimaryPointCreate(NULL, &InOutSlider_OnPrimaryCreate);
		_inOutSlider->RegisterPrimaryPointDestroy(NULL, &InOutSlider_OnPrimaryDestroy);
		_inOutSlider->RegisterValueChange(NULL, &InOutSlider_OnValueChange);
		_inOutSlider->SetValueChangeOnOffAxis(false);
	}
	
	// Feature TrackPad.
	if(_featureTrackPad)
	{
		// Track Pad.
		if(trackpad_columns > 0 && trackpad_rows > 0)
		{
			_trackPad = new XnVSelectableSlider2D(trackpad_columns, trackpad_rows);
		}
		else
		{
			_trackPad = new XnVSelectableSlider2D(4, 9);
		}
		
		_trackPad->RegisterItemHover(NULL, &TrackPad_ItemHover);
		_trackPad->RegisterItemSelect(NULL, &TrackPad_ItemSelect);
	    _trackPad->RegisterPrimaryPointCreate(NULL, &TrackPad_PrimaryCreate);
	  	_trackPad->RegisterPrimaryPointDestroy(NULL, &TrackPad_PrimaryDestroy);
	}
	
	// Feature User Tracking.
	if(_featureUserTracking)
	{
		// Setup user generator callbacks.
		XnCallbackHandle hUserCallbacks, hCalibrationCallbacks, hPoseCallbacks;
		if (!_userGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON))
		{
			printf("AS3OpenNI :: Supplied user generator doesn't support skeleton\n");
			return 1;
		}
		_userGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);
		
		// Setup Skeleton detection.
		_userGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks);
		if (_userGenerator.GetSkeletonCap().NeedPoseForCalibration())
		{
			_needPose = true;
			if (!_userGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION))
			{
				printf("AS3OpenNI :: Pose required, but not supported\n");
				return 1;
			}
			_userGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks);
			_userGenerator.GetSkeletonCap().GetCalibrationPose(_strPose);
		}
		_userGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
	}
	
	// Create the broadcaster manager.
	_broadcaster = new XnVBroadcaster();
	
	// Start generating all.
	_context.StartGeneratingAll();
	
	// Set the frame rate.
	_status = xnFPSInit(&xnFPS, 180);
	CHECK_RC(_status, "AS3OpenNI :: FPS Init");
	
	//----------------------------------------------------------------------//
	//------------------------- SETUP DISPLAY SUPPORT ---------------------//
	//--------------------------------------------------------------------//
	
	// Setup depth and image data.
	_depth.GetMetaData(_depthData);
	_image.GetMetaData(_imageData);
	
	// Hybrid mode isn't supported in this sample
	if (_imageData.FullXRes() != _depthData.FullXRes() || _imageData.FullYRes() != _depthData.FullYRes())
	{
		printf ("AS3OpenNI :: The device depth and image resolution must be equal!\n");
		return 1;
	}

	// RGB is the only image format supported.
	if (_imageData.PixelFormat() != XN_PIXEL_FORMAT_RGB24)
	{
		printf("AS3OpenNI :: The device image format must be RGB24\n");
		return 1;
	}
	
	// Setup the view points to match between the depth and image maps.
	if(_snapPixels) _depth.GetAlternativeViewPointCap().SetViewPoint(_image);
	
	//-------------------------------------------------------------//
	//------------------------- MAIN LOOP ------------------------//
	//-----------------------------------------------------------//
	
	// Setup the capture socket server for PC.
	#if (XN_PLATFORM == XN_PLATFORM_WIN32)
		if(_featureDepthMapCapture || _featureRGBCapture || _featureUserTracking)
		{
			if(_useSockets)
			{
				g_AS3Network = network();
				g_AS3Network.init(setupServer);
			}
		}
	#endif
	
	// Main loop
	while ((!_kbhit()) && (!_quit))
	{
		xnFPSMarkFrame(&xnFPS);
		_context.WaitAndUpdateAll();
		_sessionManager->Update(&_context);
		if(_featureDepthMapCapture) captureDepthMap(g_ucDepthBuffer);
		if(_featureRGBCapture) captureRGB(g_ucImageBuffer);
		#if (XN_PLATFORM == XN_PLATFORM_WIN32)
			if(_featureUserTracking) getPlayers();
		#else
			if(_featureUserTracking) renderSkeleton();
		#endif
	}
	
	CleanupExit();
}
int main()
{
	XnStatus nRetVal = XN_STATUS_OK;

	Context context;
	ScriptNode scriptNode;
	EnumerationErrors errors;

    XnUInt32 min_z1, min_z2, min_z3, maxGrad, distVal;

	const char *fn = NULL;
	if	(fileExists(SAMPLE_XML_PATH)) fn = SAMPLE_XML_PATH;
	else if (fileExists(SAMPLE_XML_PATH_LOCAL)) fn = SAMPLE_XML_PATH_LOCAL;
	else {
		printf("Could not find '%s' nor '%s'. Aborting.\n" , SAMPLE_XML_PATH, SAMPLE_XML_PATH_LOCAL);
		return XN_STATUS_ERROR;
	}
	printf("Reading config from: '%s'\n", fn);
	nRetVal = context.InitFromXmlFile(fn, scriptNode, &errors);

	if (nRetVal == XN_STATUS_NO_NODE_PRESENT)
	{
		XnChar strError[1024];
		errors.ToString(strError, 1024);
		printf("%s\n", strError);
		return (nRetVal);
	}
	else if (nRetVal != XN_STATUS_OK)
	{
		printf("Open failed: %s\n", xnGetStatusString(nRetVal));
		return (nRetVal);
	}

	DepthGenerator depth;
	nRetVal = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth);
	CHECK_RC(nRetVal, "Find depth generator");

	XnFPSData xnFPS;
	nRetVal = xnFPSInit(&xnFPS, 180);
	CHECK_RC(nRetVal, "FPS Init");

	DepthMetaData depthMD;

	//Initialize WiringPi
	if(wiringPiSetup() == -1)
		exit(1);

	//Enable SoftPWM on pin 1,2 and 3
	softPwmCreate(1, 0, RANGE);
	softPwmCreate(2, 0, RANGE);
	softPwmCreate(3, 0, RANGE);

	while (!xnOSWasKeyboardHit())
	{
		nRetVal = context.WaitOneUpdateAll(depth);
		if (nRetVal != XN_STATUS_OK)
		{
			printf("UpdateData failed: %s\n", xnGetStatusString(nRetVal));
			continue;
		}

		xnFPSMarkFrame(&xnFPS);

		depth.GetMetaData(depthMD);
		const XnDepthPixel* pDepthMap = depthMD.Data();
		int XRes = depthMD.XRes();
		int YRes = depthMD.YRes();

		//To find closest pixel value in Zone 1, Zone 2 and Zone 3
		min_z1    = getClosestPixel(  0        , 0, (XRes / 2)    , YRes, depthMD);
		min_z2    = getClosestPixel( (XRes / 4), 0, (3 * XRes / 4), YRes, depthMD);
		min_z3    = getClosestPixel( (XRes / 2), 0,  XRes         , YRes, depthMD);

		double in_low = 600;
		double in_high = 2000;
		double in_diff = in_high - in_low;
		double out_low = 51;
		double out_high = 973;
		double out_diff = out_high - out_low;

		distVal = min_z1;
		XnUInt32 pwm_val1 = ( (out_diff) / ((in_diff)*(in_diff)*(in_diff)) ) * ((in_high - distVal) * (in_high - distVal) * (in_high - distVal)) + out_low;
		distVal = min_z2;
		XnUInt32 pwm_val2 = ( (out_diff) / ((in_diff)*(in_diff)*(in_diff)) ) * ((in_high - distVal) * (in_high - distVal) * (in_high - distVal)) + out_low;
		distVal = min_z3;
		XnUInt32 pwm_val3 = ( (out_diff) / ((in_diff)*(in_diff)*(in_diff)) ) * ((in_high - distVal) * (in_high - distVal) * (in_high - distVal)) + out_low;

		// Zone 1 - Left side (pin )

		if (pwm_val1 < out_low)
			pwm_val1 = 0;  		 // if object too far, set DUTY CYCLE to 0
		if (min_z1 == 9000.0)
			pwm_val1 = out_high; //if object too close, set DUTY CYCLE to max (here, 95%)
		if (min_z1 < 600)
			pwm_val1 = out_high;

		// Zone 2 - Center (pin )

		if (pwm_val2 < out_low)
			pwm_val2 = 0;  		 // if object too far, set DUTY CYCLE to 0
		if (min_z2 == 9000.0)
			pwm_val2 = out_high; //if object too close, set DUTY CYCLE to max (here, 95%)
		if (min_z2 < 600)
			pwm_val2 = out_high;

		// Zone 3 - Right side (pin )

		if (pwm_val3 < out_low)
			pwm_val3 = 0;  		 // if object too far, set DUTY CYCLE to 0
		if (min_z3 == 9000.0)
			pwm_val3 = out_high; //if object too close, set DUTY CYCLE to max (here, 95%)
		if (min_z3 < 600)
			pwm_val3 = out_high;

		pwm_val1 = ((pwm_val1 - out_low) / (1.0 * out_diff)) * 100.0;
		pwm_val2 = ((pwm_val2 - out_low) / (1.0 * out_diff)) * 100.0;
		pwm_val3 = ((pwm_val3 - out_low) / (1.0 * out_diff)) * 100.0;

		softPwmWrite(1,(int)pwm_val1);
		softPwmWrite(2,(int)pwm_val2);
		softPwmWrite(3,(int)pwm_val3);

		if ( (depthMD.FrameID() % 30) == 0)
		{
			printf("Frame %d", depthMD.FrameID());
			printf("\n");

			printf("Zone 1 value is %u \t", pwm_val1);
			printf("Zone 2 value is %u \t", pwm_val2);
			printf("Zone 3 value is %u \n", pwm_val3);

			printf("Zone1 min_dis   %u \t", min_z1);
			printf("Zone2 min_dis   %u \t", min_z2);
			printf("Zone3 min_dis   %u \n", min_z3);


		//To find a gradient value for the floor
		//maxGrad = getGradient( 5, 0, (YRes/2) + 1, depthMD.XRes(), depthMD.YRes(), depthMD);
		//printf("Frame %d max gradient for Floor is: %u. FPS: %f\n\n", depthMD.FrameID(), maxGrad, xnFPSCalc(&xnFPS));
		}

	}

	softPwmWrite(1,0);
	softPwmWrite(2,0);
	softPwmWrite(3,0);

	//release the nodes
	depth.Release();
	scriptNode.Release();
	context.Release();



	return 0;
}
예제 #28
0
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	mxArray *tmpContext;
	mxArray *tmpIR, *tmpIRMD;
	mxArray *tmpDepth, *tmpDepthMD;
	mxArray *context_initialised_array;
	mxArray *has_ir_node_array;
	mxArray *has_depth_node_array;

	Context* context;

	IRGenerator* ir;
	IRMetaData* irMD;

	DepthGenerator* depth;
	DepthMetaData* depthMD;

	unsigned short* output_ir = 0;
	unsigned short* output_depth = 0;

	mwSize dims2_ir[2];
	mwSize dims2_depth[2];
	
	bool context_initialised = false;
	bool has_ir_node = false;
	bool has_depth_node = false;

    //---------------------------------
    // Read input variables
    //---------------------------------
    // ni_context_obj
	tmpContext = mxGetField(prhs[0], 0, "ni_context_obj");
    memcpy((void*)&context, mxGetPr(tmpContext), sizeof(Context*));

    // ir_obj
	tmpIR = mxGetField(prhs[0], 0, "ir_obj");
    memcpy((void*)&ir, mxGetPr(tmpIR), sizeof(IRGenerator*));

    // irMD_obj
	tmpIRMD = mxGetField(prhs[0], 0, "irMD_obj");
    memcpy((void*)&irMD, mxGetPr(tmpIRMD), sizeof(IRMetaData*));

    // depth_obj
	tmpDepth = mxGetField(prhs[0], 0, "depth_obj");
    memcpy((void*)&depth, mxGetPr(tmpDepth), sizeof(DepthGenerator*));

    // depthMD_obj
	tmpDepthMD = mxGetField(prhs[0], 0, "depthMD_obj");
    memcpy((void*)&depthMD, mxGetPr(tmpDepthMD), sizeof(DepthMetaData*));

	// context_initialised
	context_initialised_array = mxGetField(prhs[0], 0, "context_initialised");
	context_initialised = mxGetScalar(context_initialised_array);

	// has_ir_node
	has_ir_node_array = mxGetField(prhs[0], 0, "has_ir_node");
	has_ir_node = mxGetScalar(has_ir_node_array);

	// has_depth_node
	has_depth_node_array = mxGetField(prhs[0], 0, "has_depth_node");
	has_depth_node = mxGetScalar(has_depth_node_array);

	//-------------------------------
	// Create output variables
	//-------------------------------	
	bool initialised = context_initialised;

	int ir_width = 640;
	int ir_height = 480;

	int depth_width = 640;
	int depth_height = 480;

	if(initialised){
		XnStatus rc;
		
		if(has_ir_node){
			ir->GetMetaData(*irMD);

			ir_width = irMD->XRes();
			ir_height = irMD->YRes();
		}

		if(has_depth_node){
			depth->GetMetaData(*depthMD);

			depth_width = depthMD->XRes();
			depth_height = depthMD->YRes();
		}
	} 

	dims2_ir[0] = ir_height;
	dims2_ir[1] = ir_width;
	plhs[0] = mxCreateNumericArray(2, dims2_ir, mxUINT16_CLASS, mxREAL);
	output_ir = (unsigned short*)mxGetPr(plhs[0]);

	if(nlhs >= 2){
		dims2_depth[0] = depth_height;
		dims2_depth[1] = depth_width;
		plhs[1] = mxCreateNumericArray(2, dims2_depth, mxUINT16_CLASS, mxREAL);
		output_depth = (unsigned short*)mxGetPr(plhs[1]);
	}
	
	if(!initialised) return;

	if(has_ir_node && output_ir != 0){
		const XnIRPixel* pIR = irMD->Data();
	
		int ir_image_size = ir_width*ir_height;
		for(int i=0;i<ir_height;i++){
			for(int j = 0;j < ir_width;j++){
				output_ir[j*ir_height+i] = pIR[i*ir_width+j];
			}
		}
	}

	if(has_depth_node && output_depth != 0){
		const XnDepthPixel* pDepth = depthMD->Data();
	
		int depth_image_size = depth_width*depth_height;
		for(int i=0;i<depth_height;i++){
			for(int j = 0;j < depth_width;j++){
				output_depth[j*depth_height+i] = pDepth[i*depth_width+j];
			}
		}
	}

}
예제 #29
0
int main()
{
	XnStatus nRetVal = XN_STATUS_OK;

	Context context;
	ScriptNode scriptNode;
	EnumerationErrors errors;

	const char *fn = NULL;
	if	(fileExists(SAMPLE_XML_PATH)) fn = SAMPLE_XML_PATH;
	else if (fileExists(SAMPLE_XML_PATH_LOCAL)) fn = SAMPLE_XML_PATH_LOCAL;
	else {
		printf("Could not find '%s' nor '%s'. Aborting.\n" , SAMPLE_XML_PATH, SAMPLE_XML_PATH_LOCAL);
		return XN_STATUS_ERROR;
	}
	printf("Reading config from: '%s'\n", fn);
	nRetVal = context.InitFromXmlFile(fn, scriptNode, &errors);

	if (nRetVal == XN_STATUS_NO_NODE_PRESENT)
	{
		XnChar strError[1024];
		errors.ToString(strError, 1024);
		printf("%s\n", strError);
		return (nRetVal);
	}
	else if (nRetVal != XN_STATUS_OK)
	{
		printf("Open failed: %s\n", xnGetStatusString(nRetVal));
		return (nRetVal);
	}

	DepthGenerator depth;
	nRetVal = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth);
	CHECK_RC(nRetVal, "Find depth generator");

	XnFPSData xnFPS;
	nRetVal = xnFPSInit(&xnFPS, 180);
	CHECK_RC(nRetVal, "FPS Init");

	DepthMetaData depthMD;

	while (!xnOSWasKeyboardHit())
	{
		nRetVal = context.WaitOneUpdateAll(depth);
		if (nRetVal != XN_STATUS_OK)
		{
			printf("UpdateData failed: %s\n", xnGetStatusString(nRetVal));
			continue;
		}

		xnFPSMarkFrame(&xnFPS);

		depth.GetMetaData(depthMD);

		printf("Frame %d Middle point is: %u. FPS: %f\n", depthMD.FrameID(), depthMD(depthMD.XRes() / 2, depthMD.YRes() / 2), xnFPSCalc(&xnFPS));
	}

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

	return 0;
}
예제 #30
0
int Init()
{
	
	XnStatus rc;

	//Make sure our image types are the same as the OpenNI image types.
	assert(sizeof(XnRGB24Pixel) == sizeof(ColorPixel));
	assert(sizeof(XnDepthPixel) == sizeof(DepthPixel));
	assert(sizeof(XnStatus) == sizeof(int));

	// Load OpenNI xml settings
	char filePath[255];
	int length = Util::Helpers::GetExeDirectory(filePath, sizeof(filePath));
	filePath[length] = '\\';
	strcpy(&filePath[length+1], SAMPLE_XML_PATH);

	EnumerationErrors errors;
	rc = deviceContext.InitFromXmlFile(filePath, &errors);
	if (rc == XN_STATUS_NO_NODE_PRESENT)
	{
		//One reason would be if Microsoft SDK is installed beside PrimeSense.  Device manager should say PrimeSense instead of Microsoft Kinect.
		
		//XnChar strError[1024];
		//errors.ToString(strError, 1024);
		//LOGE("%s\n", strError);
		return -1;
	}
	else if (rc != XN_STATUS_OK)
	{
		fprintf(stderr, "%s\n", xnGetStatusString(rc));
		/*LOGE("Open failed: %s\n", xnGetStatusString(rc));*/
		return (rc);
	}

	// Retrieve colour and depth nodes
	rc = deviceContext.FindExistingNode(XN_NODE_TYPE_IMAGE, colorImageGenerator);
	rc = deviceContext.FindExistingNode(XN_NODE_TYPE_DEPTH, depthImageGenerator);

	// Set mirror mode to off
	SetMirrorMode(false);

	// Get a frame to perform checks on it
	ImageMetaData colorImageMetaData;
	DepthMetaData depthImageMetaData;
	depthImageGenerator.GetMetaData(depthImageMetaData);
	colorImageGenerator.GetMetaData(colorImageMetaData);

	// Hybrid mode isn't supported in this sample
	if (colorImageMetaData.FullXRes() != depthImageMetaData.FullXRes() || colorImageMetaData.FullYRes() != depthImageMetaData.FullYRes())
	{
		/*LOGE("The device depth and image resolution must be equal!\n");*/
		return 1;
	}

	// RGB is the only image format supported.
	if (colorImageMetaData.PixelFormat() != XN_PIXEL_FORMAT_RGB24)
	{
		/*LOGE("The device image format must be RGB24\n");*/
		return 1;
	}
	
	// Need to make sure the automatic alignment of colour and depth images is supported.
	XnBool isSupported = depthImageGenerator.IsCapabilitySupported("AlternativeViewPoint");
	if(!isSupported)
	{
		/*LOGE("Cannot set AlternativeViewPoint!\n");*/
		return 1;
	}

	
	// Set it to VGA maps at 30 FPS
	/*XnMapOutputMode mapMode;
	mapMode.nXRes = XN_VGA_X_RES;
	mapMode.nYRes = XN_VGA_Y_RES;
	mapMode.nFPS = 60;
	rc = g_depth.SetMapOutputMode(mapMode);
	if(rc)
	{
		LOGE("Failed to set depth map mode: %s\n", xnGetStatusString(rc));
		return 1;
	}
	mapMode.nFPS = 30;
	rc = g_image.SetMapOutputMode(mapMode);
	if(rc)
	{
		LOGE("Failed to set image map mode: %s\n", xnGetStatusString(rc));
		return 1;
	}*/


	// Set automatic alignment of the colour and depth images.
	rc = depthImageGenerator.GetAlternativeViewPointCap().SetViewPoint(colorImageGenerator);
	if(rc)
	{
		/*LOGE("Failed to set depth map mode: %s\n", xnGetStatusString(rc));*/
		return 1;
	}


	return XN_STATUS_OK;
}