// -----------------------------------------------------------------------------------------------------
// 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;
}
void captureRGB(unsigned char* g_ucImageBuffer)
{
ImageMetaData imd;
_image.GetMetaData(imd);
unsigned int nValue = 0;
unsigned int nX = 0;
unsigned int nY = 0;
XnUInt16 g_nXRes = imd.XRes();
XnUInt16 g_nYRes = imd.YRes();
const XnRGB24Pixel * pImageMap = _image.GetRGB24ImageMap();
for (nY=0; nY<g_nYRes; nY++)
{
for (nX=0; nX < g_nXRes; nX++)
{
((unsigned char*)g_ucImageBuffer)[(nY*g_nXRes+nX)*4+0] = pImageMap[nY*g_nXRes+nX].nBlue;
((unsigned char*)g_ucImageBuffer)[(nY*g_nXRes+nX)*4+1] = pImageMap[nY*g_nXRes+nX].nGreen;
((unsigned char*)g_ucImageBuffer)[(nY*g_nXRes+nX)*4+2] = pImageMap[nY*g_nXRes+nX].nRed;
((unsigned char*)g_ucImageBuffer)[(nY*g_nXRes+nX)*4+3] = 0x00;
}
}
}
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();
}
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;
}