Beispiel #1
0
void calculateHistogram()
{
	DepthGenerator* pDepthGen = getDepthGenerator();

	if (pDepthGen == NULL)
		return;

	XnUInt32 nZRes = pDepthGen->GetDeviceMaxDepth() + 1;
	if (g_pDepthHist == NULL)
	{
		g_pDepthHist = new float[nZRes];
	}

	xnOSMemSet(g_pDepthHist, 0, nZRes*sizeof(float));
	int nNumberOfPoints = 0;

	XnDepthPixel nValue;

	const XnDepthPixel* pDepth = pDepthGen->GetDepthMap();
	const XnDepthPixel* pDepthEnd = pDepth + (pDepthGen->GetDataSize() / sizeof(XnDepthPixel));

	while (pDepth != pDepthEnd)
	{
		nValue = *pDepth;

		XN_ASSERT(nValue <= nZRes);

		if (nValue != 0)
		{
			g_pDepthHist[nValue]++;
			nNumberOfPoints++;
		}

		pDepth++;
	}

	XnUInt32 nIndex;
	for (nIndex = 1; nIndex < nZRes; nIndex++)
	{
		g_pDepthHist[nIndex] += g_pDepthHist[nIndex-1];
	}
	for (nIndex = 1; nIndex < nZRes; nIndex++)
	{
		if (g_pDepthHist[nIndex] != 0)
		{
			g_pDepthHist[nIndex] = (nNumberOfPoints-g_pDepthHist[nIndex]) / nNumberOfPoints;
		}
	}
}
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();

}
Beispiel #3
0
void mixRGB_Depth()
{
	bool bShouldRun = true;
	int c;

	XnStatus nRetVal = XN_STATUS_OK;
	Context context;

	// Initialize context object
	nRetVal = context.Init();

	// Check error code
	if (nRetVal)
		printf("Error: %s", xnGetStatusString(nRetVal));

	context.SetGlobalMirror(true);

	//Create Depth generator node
	DepthGenerator depth;
	nRetVal = depth.Create(context);
	// Check error code
	if (nRetVal)
		printf("Error: %s", xnGetStatusString(nRetVal));

	// Create an ImageGenetor node
	ImageGenerator image;
	nRetVal = image.Create(context);
	if (nRetVal)
		printf("Error: %s", xnGetStatusString(nRetVal));

	// Sync the DepthGenerator with the ImageGenerator
	nRetVal = depth.GetAlternativeViewPointCap().SetViewPoint(image);
	if (nRetVal)
		printf("Error: %s", xnGetStatusString(nRetVal));

	//Set it to VGA maps at 30 fps
	XnMapOutputMode mapMode;
	mapMode.nXRes = XN_VGA_X_RES;
	mapMode.nYRes = XN_VGA_Y_RES;
	mapMode.nFPS = 30;
	nRetVal = depth.SetMapOutputMode(mapMode);

	// Make it start generating data
	nRetVal = context.StartGeneratingAll();
	if (nRetVal)
		printf("Error: %s", xnGetStatusString(nRetVal));

	// Create an OpenCv matrix
	CvMat* depthMetersMat = cvCreateMat(480, 640, CV_16UC1);
	IplImage *kinectDepthImage;
	kinectDepthImage = cvCreateImage(cvSize(640,480), 16, 1);

	IplImage *rgbimg = cvCreateImageHeader(cvSize(640,480), 8,3);

	// Main loop
	while (bShouldRun)
	{
		//wait for new data to be available
		nRetVal = context.WaitOneUpdateAll(depth);
		if (nRetVal)
		{
			printf("Error: %s", xnGetStatusString(nRetVal));
			continue;
		}
		//Take current depth map
		const XnDepthPixel* pDepthMap = depth.GetDepthMap();

		for (int y=0; y<XN_VGA_Y_RES; y++)
		{
			for (int x=0; x<XN_VGA_X_RES; x++)
			{
				depthMetersMat->data.s[y*XN_VGA_X_RES+x]=10*pDepthMap[y*XN_VGA_X_RES+x];
			}
		}

		cvGetImage(depthMetersMat, kinectDepthImage);

		//take current image
		const XnRGB24Pixel* pImage = image.GetRGB24ImageMap();
		//process image data
		XnRGB24Pixel* ucpImage = const_cast<XnRGB24Pixel*>(pImage);
		cvSetData(rgbimg, ucpImage, 640*3);
		cvShowImage("RGB", kinectDepthImage);

		c = cvWaitKey(1);
		if (c == 27)
			bShouldRun = false;
	}

	cvReleaseImageHeader(&kinectDepthImage);
	context.Shutdown();
}
Beispiel #4
0
void OneKinect(int type)
{
	NodeInfoList image_node_info_list;
	NodeInfoList depth_node_info_list;
	XnStatus status;
	Context context;
	int c;
	IplImage* kinectRGBImage;
	bool bShouldrun = true;

	context.Init();
//	status = context.InitFromXmlFile("D:\\initXml.xml");
	Query query;
	
	switch (type) {

	case 0: 
		status = query.SetVendor("PrimeSense");
		status = context.EnumerateProductionTrees(XN_NODE_TYPE_DEPTH, &query, depth_node_info_list, NULL);
		//status = context.EnumerateProductionTrees(XN_NODE_TYPE_DEPTH, NULL, depth_node_info_list, NULL);
		if (status != XN_STATUS_OK)
			printf("Enumerating devices failed. Reason: %s", xnGetStatusString(status));
		else
		{
			NodeInfoList::Iterator nodeIt = depth_node_info_list.Begin();
//			NodeInfo& selectedNode = *depth_node_info_list.Begin();
//			nodeIt++;
			
			NodeInfo& selectedNode = *nodeIt;

			printf("instance %s\n", selectedNode.GetInstanceName());

			DepthGenerator depth;
			status = selectedNode.GetInstance(depth);
			status = context.CreateProductionTree(selectedNode);
			status = depth.Create(context);
			status = context.StartGeneratingAll();
			cvNamedWindow("Depth", 1);

			// Create an OpenCv matrix
			CvMat* depthMetersMat = cvCreateMat(480, 640, CV_16UC1);
			IplImage *kinectDepthImage;

			while (bShouldrun)
			{
				status = context.WaitOneUpdateAll(depth);
				if (status)
				{
					printf("Error: %s", xnGetStatusString(status));
					continue;
				}
				//Take current depth map
				const XnDepthPixel* pDepthMap = depth.GetDepthMap();
				for (int y=0; y<XN_VGA_Y_RES; y++)
				{
					for (int x=0; x<XN_VGA_X_RES; x++)
					{
						depthMetersMat->data.s[y*XN_VGA_X_RES+x]=10*pDepthMap[y*XN_VGA_X_RES+x];
					}
				}
				kinectDepthImage = cvCreateImage(cvSize(640,480),8,1);
				cvGetImage(depthMetersMat, kinectDepthImage);
				cvShowImage("Depth", kinectDepthImage);
				cvReleaseImageHeader(&kinectDepthImage);
				c = cvWaitKey(1);
				if (c == 27)
					bShouldrun = false;
			}

		}
		break;
		
	case 1:
		status = context.EnumerateProductionTrees(XN_NODE_TYPE_IMAGE, NULL, image_node_info_list, NULL); 
		if (status != XN_STATUS_OK)
			printf("Enumerating devices failed. Reason: %s", xnGetStatusString(status));
		else
		{
			NodeInfo& selectedNode = *image_node_info_list.Begin();
			xn::ImageGenerator rgb;
			status = selectedNode.GetInstance(rgb);
			status = context.CreateProductionTree(selectedNode);
			status = rgb.Create(context);
			
			status = context.StartGeneratingAll();
			cvNamedWindow("RGB", 1);

			while (bShouldrun)
			{
				kinectRGBImage = cvCreateImage(cvSize(640,480),8,3);
				// Wait for new data to be available
				status = context.WaitOneUpdateAll(rgb);
				if (status != XN_STATUS_OK)
				{
					printf("Failed updating data: %s\n");
					xnGetStatusString(status);
					continue;
				}
				// Take current rgb map
				const XnRGB24Pixel* pImageMap = rgb.GetRGB24ImageMap();
				
				for (int y=0; y<XN_VGA_Y_RES; y++)
				{
					uchar *ptr = (uchar*)kinectRGBImage->imageData + y*kinectRGBImage->widthStep;
					for (int x=0; x<XN_VGA_X_RES; x++)
					{
						ptr[3*x] = pImageMap->nBlue;
						ptr[3*x + 1] = pImageMap->nGreen;
						ptr[3*x + 2] = pImageMap->nRed;
						pImageMap++;
					}
				}
				cvShowImage("RGB", kinectRGBImage);
				cvReleaseImageHeader(&kinectRGBImage);
				c = cvWaitKey(1);
				if (c == 27)
					bShouldrun = false;
			}
		}
		break;

	default:
		cout << "Incorrect number" << endl;

	} // end switch

	// Clean-up
	context.Shutdown();

}
void main()
{
	//--------------------------------------------------- 
	bool bShouldRun = true;
	int c ;

	XnStatus nRetVal = XN_STATUS_OK;
	Context context;

	// Initialize context object
	nRetVal = context.Init();

	// check error code
	if(nRetVal)
		printf("Error : %s", xnGetStatusString(nRetVal));

	context.SetGlobalMirror(true); //mirror image

	// Create a DepthGenerator node
	DepthGenerator depth;
	nRetVal = depth.Create(context);
	// check error code
	if(nRetVal)
		printf("Failed to create depth generator: %s\n", xnGetStatusString(nRetVal));

	/// Create an ImageGenerator node
	ImageGenerator image;
	nRetVal = image.Create(context);
	if(nRetVal)
		printf("Failed to create image generator: %s\n", xnGetStatusString(nRetVal));

	if(nRetVal)
		printf("Failed to match Depth and RGB points of view: %s\n",xnGetStatusString(nRetVal));

	// Set it to VGA maps at 30 FPS
	XnMapOutputMode mapMode;
	mapMode.nXRes = XN_VGA_X_RES;
	mapMode.nYRes = XN_VGA_Y_RES;
	mapMode.nFPS = 30;
	nRetVal = depth.SetMapOutputMode(mapMode);

	// Make it start generating data
	nRetVal = context.StartGeneratingAll();

	//  check error code
	if(nRetVal)
		printf("Error : %s", xnGetStatusString(nRetVal));

	//create a OpenCv matrix
	CvMat* depthMetersMat = cvCreateMat(480, 640, CV_16UC1);
	IplImage *kinectDepthImage;
	kinectDepthImage = cvCreateImage( cvSize(640,480),16,1);
	IplImage *kinectDepthImage_raw= cvCreateImage( cvSize(640,480),16,1);

	IplImage rgbimg;

	XnPoint3D* pDepthPointSet = new XnPoint3D[ 640*480 ];


	// Main loop

	while (bShouldRun)
	{
		// Wait for new data to be available
		nRetVal = context.WaitOneUpdateAll(depth);

		if (nRetVal != XN_STATUS_OK)
		{
			printf("Failed updating data: %s\n", xnGetStatusString(nRetVal));
			continue;
		}

		// Take current depth map
		const XnDepthPixel* pDepthMap = depth.GetDepthMap();
		xn::DepthGenerator rDepth;

		//Copy the depth values
		for (int y=0; y<XN_VGA_Y_RES; y++)
			for(int x=0;x<XN_VGA_X_RES;x++)
			{
				depthMetersMat->data.s[y * XN_VGA_X_RES + x ] = 20*pDepthMap[y * XN_VGA_X_RES + x];

				// Convert the coordinates in the camera coordinate system
				pDepthPointSet[y * XN_VGA_X_RES + x].X = (XnFloat) x;
				pDepthPointSet[y * XN_VGA_X_RES + x].Y = (XnFloat) y;
				pDepthPointSet[y * XN_VGA_X_RES + x].Z = pDepthMap[y * XN_VGA_X_RES + x];

			}
		
			cvGetImage(depthMetersMat,kinectDepthImage_raw);
			cvShowImage("Depth stream", kinectDepthImage_raw);

			unsigned char* picture_RGB = new unsigned char[XN_VGA_X_RES * XN_VGA_Y_RES * 3];
			//initialization with the retrieved data
			memcpy(picture_RGB, (unsigned char*)image.GetRGB24ImageMap(),XN_VGA_Y_RES * XN_VGA_X_RES * 3);

			//From BGR to RGB
			for(int i = 0 ; i < XN_VGA_X_RES * XN_VGA_Y_RES ; i++)
			{   
				unsigned char temp = picture_RGB[i*3];
				picture_RGB[i*3] = picture_RGB[i*3+2];
				picture_RGB[i*3+2] = temp;
			}
			cv::Mat colorMatRes(XN_VGA_Y_RES, XN_VGA_X_RES, CV_8UC3, picture_RGB);
			rgbimg=colorMatRes; //Conversion from cv::mat to IplImage format
			cvShowImage("Color stream",&rgbimg); //Display the RGB stream

			// free memory 
			delete picture_RGB;
			c = cvWaitKey(1);
			if (c == 27)
				bShouldRun = false; //exit main loop

	}

	// Clean-up
	cvDestroyWindow("Color stream"); 
	cvDestroyWindow("Depth stream"); 
	cvReleaseImageHeader(&kinectDepthImage);
	delete pDepthPointSet;
	
}