Beispiel #1
0
inline void getBGRImageFromMetaData( const xn::ImageMetaData& imageMetaData, cv::Mat& bgrImage )
{
    if( imageMetaData.PixelFormat() != XN_PIXEL_FORMAT_RGB24 )
        CV_Error( CV_StsUnsupportedFormat, "Unsupported format of grabbed image\n" );

    cv::Mat rgbImage( imageMetaData.YRes(), imageMetaData.XRes(), CV_8UC3 );
    const XnRGB24Pixel* pRgbImage = imageMetaData.RGB24Data();

    // CV_Assert( 3*sizeof(uchar) == sizeof(XnRGB24Pixel) );
    memcpy( rgbImage.data, pRgbImage, rgbImage.total()*sizeof(XnRGB24Pixel) );
    cv::cvtColor( rgbImage, bgrImage, CV_RGB2BGR );
}
void SimKinect::DrawRGBMap(const xn::ImageMetaData& imd) {
	static bool bInitialized = false;
	static int width;
	static int height;
	static int width_step;
	static XnRGB24Pixel* g_pTexMap;

	if(!bInitialized) {
		width = video_size_width;
		height = video_size_height;
		g_pTexMap = (XnRGB24Pixel*)malloc(width * height * sizeof(XnRGB24Pixel));
		width_step = width*3;
		bInitialized = true;
	}

	const XnUInt8* pImage = imd.Data();
	const XnRGB24Pixel* pImageRow = imd.RGB24Data();
	XnRGB24Pixel* pTexRow = g_pTexMap + imd.YOffset() * width;
	
	for (XnUInt y = 0; y < imd.YRes(); ++y)
	{
		const XnRGB24Pixel* pImage = pImageRow;
		XnRGB24Pixel* pTex = pTexRow + imd.XOffset();

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

		pImageRow += imd.XRes();
		pTexRow += width;
	}

	//Draw rgb image
  XnRGB24Pixel* pix_ptr = g_pTexMap;
  
  for ( int i=0 ; i<height ; i++ ) {
		unsigned char* row_pointer = color_frame+i*width_step;
    for ( int j=0 ; j<width_step; j+=3,pix_ptr++ ) {
			row_pointer[j] = pix_ptr->nBlue;
      row_pointer[j+1]=pix_ptr->nGreen;
      row_pointer[j+2]=pix_ptr->nRed;
    }
  }
	//imshow("COLOR",color_img);
	
}
Beispiel #3
0
bool DataCapture::captureOne()
{
    XnStatus rc = context_.WaitAndUpdateAll(); // want this to be WaitOneUpdateAll(RGB image)
    if( rc != XN_STATUS_OK )
    {
        std::cout << "WaitAndUpdateAll: " << xnGetStatusString(rc) << std::endl;
        return false;
    }

    // grab image
    imageGen_.GetMetaData(imageMd_);
    const XnRGB24Pixel* rgbData = imageMd_.RGB24Data();
    for( unsigned int i = 0; i < 640 * 480; ++i )
    {
        pRgbData_[3*i] = rgbData->nRed;
        pRgbData_[3*i + 1] = rgbData->nGreen;
        pRgbData_[3*i + 2] = rgbData->nBlue;
        ++rgbData;
    }

    // grab depth image
    depthGen_.GetMetaData(depthMd_);
    const uint16_t* pDepthDataU16 = depthMd_.Data();
    for( int i = 0; i < 640 * 480; ++i)
    {
        uint16_t d = pDepthDataU16[i];
        if( d != 0 )
        {
            pDepthData_[i] = (d * 255)/2048;
        }
        else
        {
            pDepthData_[i] = 0; // should be NAN
        }
    }
    return true;
}
Beispiel #4
0
void start_kinect() {
    XnStatus nRetVal = XN_STATUS_OK;
    xn::EnumerationErrors errors;
    UsersCount = 0;

    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 = g_Context.InitFromXmlFile(fn, g_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);
    }

    nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_depth);      
    CHECK_RC(nRetVal,"No depth");
    
	  nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_image);      
    CHECK_RC(nRetVal,"No image");

    nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator);
    if (nRetVal != XN_STATUS_OK)
    {
        nRetVal = g_UserGenerator.Create(g_Context);
        CHECK_RC(nRetVal, "Find user generator");
    }

    XnCallbackHandle hUserCallbacks, hCalibrationStart, hCalibrationComplete, hPoseDetected;
    if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON))
    {
        printf("Supplied user generator doesn't support skeleton\n");
        //return 1;
    }
    nRetVal = g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);
    CHECK_RC(nRetVal, "Register to user callbacks");
    nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationStart(UserCalibration_CalibrationStart, NULL, hCalibrationStart);
    CHECK_RC(nRetVal, "Register to calibration start");
    nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationComplete(UserCalibration_CalibrationComplete, NULL, hCalibrationComplete);
    CHECK_RC(nRetVal, "Register to calibration complete");

    if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration())
    {
        g_bNeedPose = TRUE;
        if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION))
        {
            printf("Pose required, but not supported\n");
            //return 1;
        }
        nRetVal = g_UserGenerator.GetPoseDetectionCap().RegisterToPoseDetected(UserPose_PoseDetected, NULL, hPoseDetected);
        CHECK_RC(nRetVal, "Register to Pose Detected");
        g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose);
    }

    g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);

    nRetVal = g_Context.StartGeneratingAll();
    CHECK_RC(nRetVal, "StartGenerating");

    XnUserID aUsers[MAX_NUM_USERS];
    XnUInt16 nUsers;

    XnSkeletonJointTransformation anyjoint;

    printf("Starting to run\n");
    if(g_bNeedPose)
    {
        printf("Assume calibration pose\n");
    }
    XnUInt32 epochTime = 0;
    while (!xnOSWasKeyboardHit())
    {
        g_Context.WaitOneUpdateAll(g_UserGenerator);
        // print the torso information for the first user already tracking
        nUsers=MAX_NUM_USERS;
        g_UserGenerator.GetUsers(aUsers, nUsers);
        int numTracked=0;
        int userToPrint=-1;

        WriteLock w_lock(myLock);
    	  pDepthMap = g_depth.GetDepthMap();
        pPixelMap = g_image.GetRGB24ImageMap();
            
    	  g_depth.GetMetaData(g_depthMD);    
        g_image.GetMetaData(g_imageMD);
        pPixelPoint = g_imageMD.RGB24Data();


        for(XnUInt16 i=0; i<nUsers; i++) {
    			if(g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])==FALSE)
    				continue;
    			{
    				
    				
    				/* Writing all new movements into structure*/
    				/* Head */
    				g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_HEAD,anyjoint);
    				Skeletons[i]["Head"]["X"] = anyjoint.position.position.X;
    				Skeletons[i]["Head"]["Y"] = anyjoint.position.position.Y;
    				Skeletons[i]["Head"]["Z"] = anyjoint.position.position.Z;
    				/* Neck */
    				g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_NECK,anyjoint);
    				Skeletons[i]["Neck"]["X"] = anyjoint.position.position.X;
    				Skeletons[i]["Neck"]["Y"] = anyjoint.position.position.Y;
    				Skeletons[i]["Neck"]["Z"] = anyjoint.position.position.Z;
    				/* Left Shoulder */
    				g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_SHOULDER,anyjoint);
    				Skeletons[i]["LeftShoulder"]["X"] = anyjoint.position.position.X;
    				Skeletons[i]["LeftShoulder"]["Y"] = anyjoint.position.position.Y;
    				Skeletons[i]["LeftShoulder"]["Z"] = anyjoint.position.position.Z;
    				/* Right Shoulder */
    				g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_SHOULDER,anyjoint);
    				Skeletons[i]["RightShoulder"]["X"] = anyjoint.position.position.X;
    				Skeletons[i]["RightShoulder"]["Y"] = anyjoint.position.position.Y;
    				Skeletons[i]["RightShoulder"]["Z"] = anyjoint.position.position.Z;
    				/* Torso */
    				g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_TORSO,anyjoint);
    				Skeletons[i]["Torso"]["X"] = anyjoint.position.position.X;
    				Skeletons[i]["Torso"]["Y"] = anyjoint.position.position.Y;
    				Skeletons[i]["Torso"]["Z"] = anyjoint.position.position.Z;
    				/* Left Elbow */
    				g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_ELBOW,anyjoint);
    				Skeletons[i]["LeftElbow"]["X"] = anyjoint.position.position.X;
    				Skeletons[i]["LeftElbow"]["Y"] = anyjoint.position.position.Y;
    				Skeletons[i]["LeftElbow"]["Z"] = anyjoint.position.position.Z;
    				/* Right Elbow */
    				g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_ELBOW,anyjoint);
    				Skeletons[i]["RightElbow"]["X"] = anyjoint.position.position.X;
    				Skeletons[i]["RightElbow"]["Y"] = anyjoint.position.position.Y;
    				Skeletons[i]["RightElbow"]["Z"] = anyjoint.position.position.Z;
    				/* Left Hip */
    				g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_HIP,anyjoint);
    				Skeletons[i]["LeftHip"]["X"] = anyjoint.position.position.X;
    				Skeletons[i]["LeftHip"]["Y"] = anyjoint.position.position.Y;
    				Skeletons[i]["LeftHip"]["Z"] = anyjoint.position.position.Z;
    				/* Right Hip */
    				g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_HIP,anyjoint);
    				Skeletons[i]["RightHip"]["X"] = anyjoint.position.position.X;
    				Skeletons[i]["RightHip"]["Y"] = anyjoint.position.position.Y;
    				Skeletons[i]["RightHip"]["Z"] = anyjoint.position.position.Z;
    				/* Left Hand */
    				g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_HAND,anyjoint);
    				Skeletons[i]["LeftHand"]["X"] = anyjoint.position.position.X;
    				Skeletons[i]["LeftHand"]["Y"] = anyjoint.position.position.Y;
    				Skeletons[i]["LeftHand"]["Z"] = anyjoint.position.position.Z;
    				/* Right Hand */
    				g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_HAND,anyjoint);
    				Skeletons[i]["RightHand"]["X"] = anyjoint.position.position.X;
    				Skeletons[i]["RightHand"]["Y"] = anyjoint.position.position.Y;
    				Skeletons[i]["RightHand"]["Z"] = anyjoint.position.position.Z;
    				/* Left Knee */
    				g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_KNEE,anyjoint);
    				Skeletons[i]["LeftKnee"]["X"] = anyjoint.position.position.X;
    				Skeletons[i]["LeftKnee"]["Y"] = anyjoint.position.position.Y;
    				Skeletons[i]["LeftKnee"]["Z"] = anyjoint.position.position.Z;
    				/* Right Knee */
    				g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_KNEE,anyjoint);
    				Skeletons[i]["RightKnee"]["X"] = anyjoint.position.position.X;
    				Skeletons[i]["RightKnee"]["Y"] = anyjoint.position.position.Y;
    				Skeletons[i]["RightKnee"]["Z"] = anyjoint.position.position.Z;
    				/* Left Foot */
    				g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_LEFT_FOOT,anyjoint);
    				Skeletons[i]["LeftFoot"]["X"] = anyjoint.position.position.X;
    				Skeletons[i]["LeftFoot"]["Y"] = anyjoint.position.position.Y;
    				Skeletons[i]["LeftFoot"]["Z"] = anyjoint.position.position.Z;
    				/* Right Foot */
    				g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_RIGHT_FOOT,anyjoint);
    				Skeletons[i]["RightFoot"]["X"] = anyjoint.position.position.X;
    				Skeletons[i]["RightFoot"]["Y"] = anyjoint.position.position.Y;
    				Skeletons[i]["RightFoot"]["Z"] = anyjoint.position.position.Z;
    
    				/*printf("user %d: head at (%6.2f,%6.2f,%6.2f)\n",aUsers[i],
                                                                      Skeletons[i]["Head"]["X"],
                                                                      Skeletons[i]["Head"]["Y"],
                                                                      Skeletons[i]["Head"]["Z"]);*/
    			}				
        }
        
    }
    g_scriptNode.Release();
    g_depth.Release();
    g_image.Release();
    g_UserGenerator.Release();
    g_Context.Release();
}
void DrawDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd, XnUserID player, xn::ImageMetaData& imd)
{
	texWidth = 640;
	texHeight = 480;


	LEFT = 0; RIGHT = 640;
	TOP = 0; BOTTOM = 480;

	nValue = 0;
	nIndex = 0;
	nX = 0; nY = 0;
	nNumberOfPoints = 0;
	g_nXRes = dmd.XRes();
	g_nYRes = dmd.YRes();

	pDestImage = pDepthTexBuf;

	pDepth = dmd.Data();
	pixel = imd.RGB24Data();
	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)));
		}
	}
//	printf("Debug: %i\n",focus);
	pDepth = (short unsigned int*)dmd.Data();
	///{
		// Prepare the texture map
		for (nY=0; nY<g_nYRes; nY++)
		{
			for (nX=0; nX < g_nXRes; nX++)
			{

				nValue = *pDepth;
				if(nX == (int)centerScreen[0] && nY == (int)centerScreen[1]){
					if (calibrationMode){
						depthVal = nValue;
//						printf("depthVal: %i\n",depthVal);
					}
				}
					//printf("Depth: %i \n",nValue);
				label = *pLabels;
//				XnUInt32 nColorID = label % nColors;
				if (label != focus)
				{
					if(calibrationMode){
						pDestImage[0] = pixel->nRed;
						pDestImage[1] = pixel->nGreen;
						pDestImage[2] = pixel->nBlue;
						pDestImage[3] = 255;
					} else {
						pDestImage[0] = 0;
						pDestImage[1] = 0;
						pDestImage[2] = 0;
						pDestImage[3] = 0;
					}
				} else {
					pDestImage[0] = pixel->nRed;
					pDestImage[1] = pixel->nGreen;
					pDestImage[2] = pixel->nBlue;
					pDestImage[3] = 255;

					//find max/min values for width and height boundaries
					if (nX > (unsigned int)LEFT) {
						LEFT = nX;
					}

					if (nX < (unsigned int)RIGHT) {
						RIGHT = nX;
					}

					if (nY > (unsigned int)TOP) {
						TOP = nY;
					}

					if (nY < (unsigned int)BOTTOM) {
						BOTTOM = nY;
					}
				}

				pixel++;
				pDepth++;
				pLabels++;
				pDestImage+=4;
			}

			pDestImage += (texWidth - g_nXRes) *4;
		}
	//}*/

	// Display the OpenGL texture map
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, texWidth, texHeight, 0, GL_RGBA, GL_UNSIGNED_BYTE, pDepthTexBuf);

	nUsers = 15;
	g_UserGenerator.GetUsers(aUsers, nUsers);
	g_UserGenerator.GetCoM(aUsers[0], com);


	CENTER = com.X;

	//glDisable(GL_BLEND);
	//glDisable(GL_TEXTURE_2D);
/*
	char strLabel[20] = "";
	XnUserID aUsers[15];
	XnUInt16 nUsers = 15;
	g_UserGenerator.GetUsers(aUsers, nUsers);
	for (int i = 0; i < nUsers; ++i)
	{
		XnPoint3D com;
		g_UserGenerator.GetCoM(aUsers[i], com);
		g_DepthGenerator.ConvertRealWorldToProjective(1, &com, &com);

		if (aUsers[i] == player)
			sprintf(strLabel, "%d (Player)", aUsers[i]);
		else
			sprintf(strLabel, "%d", aUsers[i]);

		//glColor4f(1-Colors[i%nColors][0], 1-Colors[i%nColors][1], 1-Colors[i%nColors][2], 1);

		glRasterPos2i(com.X, com.Y);
		glPrintString(GLUT_BITMAP_HELVETICA_18, strLabel); */
//	}

	// Draw skeleton of user
//		glBegin(GL_LINES);
//		//glColor4f(1,0,0,1.0f);
//		glVertex3f(-1, 1, 0);
//		glVertex3f(1, -1, 0);
//		//glColor4f(1-Colors[player%nColors][0], 1-Colors[player%nColors][1], 1-Colors[player%nColors][2], 1);
//		DrawLimb(1, XN_SKEL_HEAD, XN_SKEL_NECK);
//
//		DrawLimb(1, XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER);
//		DrawLimb(1, XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW);
//		DrawLimb(1, XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND);
//
//		DrawLimb(1, XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER);
//		DrawLimb(1, XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW);
//		DrawLimb(1, XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND);
//
//		DrawLimb(1, XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO);
//		DrawLimb(1, XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO);
//
//		DrawLimb(1, XN_SKEL_TORSO, XN_SKEL_LEFT_HIP);
//		DrawLimb(1, XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE);
//		DrawLimb(1, XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT);
//
//		DrawLimb(1, XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP);
//		DrawLimb(1, XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE);
//		DrawLimb(1, XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT);
//		glEnd();

	//XnSkeletonJoint eJoint1 = XN_SKEL_RIGHT_HAND;

}