void captureSingleFrame(int)
{
int num = findUniqueFileName();
XnChar csColorFileName[XN_FILE_MAX_PATH];
XnChar csDepthFileName[XN_FILE_MAX_PATH];
XnChar csIRFileName[XN_FILE_MAX_PATH];
getColorFileName(num, csColorFileName);
getDepthFileName(num, csDepthFileName);
getIRFileName(num, csIRFileName);
openni::VideoFrameRef& colorFrame = getColorFrame();
if (colorFrame.isValid())
{
xnOSSaveFile(csColorFileName, colorFrame.getData(), colorFrame.getDataSize());
}
openni::VideoFrameRef& depthFrame = getDepthFrame();
if (depthFrame.isValid())
{
xnOSSaveFile(csDepthFileName, depthFrame.getData(), depthFrame.getDataSize());
}
openni::VideoFrameRef& irFrame = getIRFrame();
if (irFrame.isValid())
{
xnOSSaveFile(csIRFileName, irFrame.getData(), irFrame.getDataSize());
}
g_Capture.nCapturedFrameUniqueID = num + 1;
displayMessage("Frames saved with ID %d", num);
}
void calculateHistogram()
{
xnOSMemSet(g_pDepthHist, 0, MAX_DEPTH*sizeof(float));
int nNumberOfPoints = 0;
openni::DepthPixel nValue;
openni::VideoStream& depthGen = getDepthStream();
if (!depthGen.isValid() || !getDepthFrame().isValid())
return;
const openni::DepthPixel* pDepth = (const openni::DepthPixel*)getDepthFrame().getData();
const openni::DepthPixel* pDepthEnd = pDepth + (getDepthFrame().getDataSize() / sizeof(openni::DepthPixel));
while (pDepth != pDepthEnd)
{
nValue = *pDepth;
XN_ASSERT(nValue <= MAX_DEPTH);
if (nValue != 0)
{
g_pDepthHist[nValue]++;
nNumberOfPoints++;
}
pDepth++;
}
XnUInt32 nIndex;
for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
{
g_pDepthHist[nIndex] += g_pDepthHist[nIndex-1];
}
for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
{
if (g_pDepthHist[nIndex] != 0)
{
g_pDepthHist[nIndex] = (nNumberOfPoints-g_pDepthHist[nIndex]) / nNumberOfPoints;
}
}
}
void drawColor(IntRect* pLocation, IntPair* pPointer, int pointerRed, int pointerGreen, int pointerBlue)
{
if (g_DrawConfig.Streams.Color.Coloring == COLOR_OFF)
return;
if (!isColorOn() && !isIROn())
{
drawClosedStream(pLocation, "Color");
return;
}
openni::VideoFrameRef colorMD;
int depthWidth = 0, depthHeight = 0, depthFullWidth = 0, depthFullHeight = 0;
int depthOriginX = 0, depthOriginY = 0;
if (isColorOn())
{
colorMD = getColorFrame();
if (!colorMD.isValid())
return;
}
else if (isIROn())
{
colorMD = getIRFrame();
}
else
return;
if (!colorMD.isValid())
return;
if (colorMD.getFrameIndex() == 0)
{
return;
}
openni::VideoFrameRef depthMetaData = getDepthFrame();
int width = colorMD.getWidth();
int height = colorMD.getHeight();
int fullWidth = colorMD.getVideoMode().getResolutionX();
int fullHeight = colorMD.getVideoMode().getResolutionY();
int originX = colorMD.getCropOriginX();
int originY = colorMD.getCropOriginY();
XnUInt8* pColor = (XnUInt8*)colorMD.getData();
bool useDepth = false;
openni::PixelFormat format = colorMD.getVideoMode().getPixelFormat();
openni::DepthPixel* pDepth = NULL;
if (depthMetaData.isValid())
{
useDepth = true;
depthWidth = depthMetaData.getWidth();
depthHeight = depthMetaData.getHeight();
depthFullWidth = depthMetaData.getVideoMode().getResolutionX();
depthFullHeight = depthMetaData.getVideoMode().getResolutionY();
depthOriginX = depthMetaData.getCropOriginX();
depthOriginY = depthMetaData.getCropOriginY();
pDepth = (openni::DepthPixel*)depthMetaData.getData();
}
for (XnUInt16 nY = 0; nY < height; nY++)
{
XnUInt8* pTexture = TextureMapGetLine(&g_texColor, nY + originY) + originX*4;
if (format == openni::PIXEL_FORMAT_YUV422)
{
YUV422ToRGB888(pColor, pTexture, width*2, g_texColor.Size.X*g_texColor.nBytesPerPixel);
pColor += width*2;
}
else if (format == openni::PIXEL_FORMAT_YUYV)
{
YUYVToRGB888(pColor, pTexture, width*2, g_texColor.Size.X*g_texColor.nBytesPerPixel);
pColor += width*2;
}
else
{
XnDouble dRealY = (nY + originY) / (XnDouble)fullHeight;
XnInt32 nDepthY = dRealY * depthFullHeight - depthOriginY;
for (XnUInt16 nX = 0; nX < width; nX++, pTexture+=4)
{
XnInt32 nDepthIndex = 0;
if (useDepth)
{
XnDouble dRealX = (nX + originX) / (XnDouble)fullWidth;
XnInt32 nDepthX = dRealX * depthFullWidth - depthOriginX;
if (nDepthX >= depthWidth || nDepthY >= depthHeight || nDepthX < 0 || nDepthY < 0)
{
nDepthIndex = -1;
}
else
{
nDepthIndex = nDepthY*depthWidth + nDepthX;
}
}
switch (format)
{
case openni::PIXEL_FORMAT_RGB888:
pTexture[0] = pColor[0];
pTexture[1] = pColor[1];
pTexture[2] = pColor[2];
pColor+=3;
break;
case openni::PIXEL_FORMAT_GRAY8:
pTexture[0] = pTexture[1] = pTexture[2] = *pColor;
pColor+=1;
break;
case openni::PIXEL_FORMAT_GRAY16:
pTexture[0] = pTexture[1] = pTexture[2] = *((XnUInt16*)pColor) >> 2;
pColor+=2;
break;
default:
assert(0);
return;
}
// decide if pixel should be lit or not
if (g_DrawConfig.Streams.Color.Coloring == DEPTH_MASKED_COLOR &&
(!depthMetaData.isValid() || nDepthIndex == -1 || pDepth[nDepthIndex] == 0))
{
pTexture[3] = 0;
}
else
{
pTexture[3] = 255;
}
}
}
}
if (pPointer != NULL)
{
TextureMapDrawCursor(&g_texColor, *pPointer, pointerRed, pointerGreen, pointerBlue);
}
TextureMapUpdate(&g_texColor);
TextureMapDraw(&g_texColor, pLocation);
}
void drawFrame()
{
// calculate locations
g_DrawConfig.DepthLocation.uBottom = 0;
g_DrawConfig.DepthLocation.uTop = WIN_SIZE_Y - 1;
g_DrawConfig.DepthLocation.uLeft = 0;
g_DrawConfig.DepthLocation.uRight = WIN_SIZE_X - 1;
g_DrawConfig.ColorLocation.uBottom = 0;
g_DrawConfig.ColorLocation.uTop = WIN_SIZE_Y - 1;
g_DrawConfig.ColorLocation.uLeft = 0;
g_DrawConfig.ColorLocation.uRight = WIN_SIZE_X - 1;
if (g_DrawConfig.Streams.ScreenArrangement == SIDE_BY_SIDE)
{
g_DrawConfig.DepthLocation.uTop = WIN_SIZE_Y / 2 - 1;
g_DrawConfig.DepthLocation.uRight = WIN_SIZE_X / 2 - 1;
g_DrawConfig.ColorLocation.uTop = WIN_SIZE_Y / 2 - 1;
g_DrawConfig.ColorLocation.uLeft = WIN_SIZE_X / 2;
}
// Texture map init
openni::VideoFrameRef* pDepthMD = &getDepthFrame();
if (isDepthOn() && pDepthMD->isValid())
{
int maxDepth = 0;
maxDepth = getDepthStream().getMaxPixelValue();
g_fMaxDepth = maxDepth;
TextureMapInit(&g_texDepth, pDepthMD->getVideoMode().getResolutionX(), pDepthMD->getVideoMode().getResolutionY(), 4, pDepthMD->getWidth(), pDepthMD->getHeight());
fixLocation(&g_DrawConfig.DepthLocation, pDepthMD->getVideoMode().getResolutionX(), pDepthMD->getVideoMode().getResolutionY());
}
openni::VideoFrameRef* pImageMD = NULL;
if (isColorOn())
{
pImageMD = &getColorFrame();
}
else if (isIROn())
{
pImageMD = &getIRFrame();
}
if (pImageMD != NULL && pImageMD->isValid())
{
TextureMapInit(&g_texColor, pImageMD->getVideoMode().getResolutionX(), pImageMD->getVideoMode().getResolutionY(), 4, pImageMD->getWidth(), pImageMD->getHeight());
fixLocation(&g_DrawConfig.ColorLocation, pImageMD->getVideoMode().getResolutionX(), pImageMD->getVideoMode().getResolutionY());
}
// check if pointer is over a map
bool bOverDepth = (pDepthMD != NULL && pDepthMD->isValid()) && isPointInRect(g_DrawUserInput.Cursor, &g_DrawConfig.DepthLocation);
bool bOverImage = (pImageMD != NULL && pImageMD->isValid()) && isPointInRect(g_DrawUserInput.Cursor, &g_DrawConfig.ColorLocation);
bool bDrawDepthPointer = false;
bool bDrawImagePointer = false;
int imagePointerRed = 255;
int imagePointerGreen = 0;
int imagePointerBlue = 0;
IntPair pointerInDepth = {0,0};
IntPair pointerInColor = {0,0};
if (bOverImage)
{
pointerInColor.X = (double)(g_DrawUserInput.Cursor.X - g_DrawConfig.ColorLocation.uLeft) / (g_DrawConfig.ColorLocation.uRight - g_DrawConfig.ColorLocation.uLeft + 1) * pImageMD->getVideoMode().getResolutionX();
pointerInColor.Y = (double)(g_DrawUserInput.Cursor.Y - g_DrawConfig.ColorLocation.uBottom) / (g_DrawConfig.ColorLocation.uTop - g_DrawConfig.ColorLocation.uBottom + 1) * pImageMD->getVideoMode().getResolutionY();
bDrawImagePointer = true;
}
if (bOverDepth)
{
pointerInDepth.X = (double)(g_DrawUserInput.Cursor.X - g_DrawConfig.DepthLocation.uLeft) / (g_DrawConfig.DepthLocation.uRight - g_DrawConfig.DepthLocation.uLeft + 1) * pDepthMD->getVideoMode().getResolutionX();
pointerInDepth.Y = (double)(g_DrawUserInput.Cursor.Y - g_DrawConfig.DepthLocation.uBottom) / (g_DrawConfig.DepthLocation.uTop - g_DrawConfig.DepthLocation.uBottom + 1) * pDepthMD->getVideoMode().getResolutionY();
bDrawDepthPointer = true;
if (!bOverImage && g_DrawConfig.bShowPointer &&
pointerInDepth.X >= pDepthMD->getCropOriginX() && pointerInDepth.X < (pDepthMD->getCropOriginX() + pDepthMD->getWidth()) &&
pointerInDepth.Y >= pDepthMD->getCropOriginY() && pointerInDepth.Y < (pDepthMD->getCropOriginY() + pDepthMD->getHeight()))
{
// try to translate depth pixel to image
openni::DepthPixel* pDepthPixels = (openni::DepthPixel*)pDepthMD->getData();
openni::DepthPixel pointerDepth = pDepthPixels[(pointerInDepth.Y - pDepthMD->getCropOriginY()) * pDepthMD->getWidth() + (pointerInDepth.X - pDepthMD->getCropOriginX())];
if (convertDepthPointToColor(pointerInDepth.X, pointerInDepth.Y, pointerDepth, &pointerInColor.X, &pointerInColor.Y))
{
bDrawImagePointer = true;
imagePointerRed = 0;
imagePointerGreen = 0;
imagePointerBlue = 255;
}
}
}
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);
// Setup the opengl env for fixed location view
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
glOrtho(0,WIN_SIZE_X,WIN_SIZE_Y,0,-1.0,1.0);
glDisable(GL_DEPTH_TEST);
if (g_DrawConfig.Streams.Depth.Coloring == CYCLIC_RAINBOW_HISTOGRAM || g_DrawConfig.Streams.Depth.Coloring == LINEAR_HISTOGRAM || g_DrawConfig.bShowPointer)
calculateHistogram();
drawColor(&g_DrawConfig.ColorLocation, bDrawImagePointer ? &pointerInColor : NULL, imagePointerRed, imagePointerGreen, imagePointerBlue);
drawDepth(&g_DrawConfig.DepthLocation, bDrawDepthPointer ? &pointerInDepth : NULL);
printRecordingInfo();
if (g_DrawConfig.bShowPointer)
drawPointerMode(bOverDepth ? &pointerInDepth : NULL);
drawUserInput(!bOverDepth && !bOverImage);
drawUserMessage();
drawPlaybackSpeed();
if (g_DrawConfig.strErrorState[0] != '\0')
drawErrorState();
if (g_DrawConfig.bHelp)
drawHelpScreen();
glutSwapBuffers();
}
void drawPointerMode(IntPair* pPointer)
{
char buf[512] = "";
XnUInt32 chars;
int nCharWidth = glutBitmapWidth(GLUT_BITMAP_HELVETICA_18, '0');
int nPointerValue = 0;
XnDouble dTimestampDivider = 1E6;
openni::VideoFrameRef* pDepthMD = &getDepthFrame();
if (pDepthMD->isValid())
{
// Print the scale black background
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glBegin(GL_QUADS);
glColor4f(0, 0, 0, 0.7);
glVertex2i(0, WIN_SIZE_Y); // lower left
glVertex2i(WIN_SIZE_X, WIN_SIZE_Y);
glVertex2i(WIN_SIZE_X, WIN_SIZE_Y - 135);
glVertex2i(0, WIN_SIZE_Y - 135);
glEnd();
glDisable(GL_BLEND);
// set a large point size (for the scale)
glPointSize(15);
// Print the scale data
glBegin(GL_POINTS);
for (int i=0; i<g_fMaxDepth; i+=1)
{
float fNewColor = g_pDepthHist[i];
if ((fNewColor > 0.004) && (fNewColor < 0.996))
{
glColor3f(fNewColor, fNewColor, 0);
glVertex3f(((i/10)*2), WIN_SIZE_Y - 23, 1);
}
}
glEnd();
// Print the pointer scale data
if (pPointer != NULL)
{
// make sure pointer in on a depth pixel (take in mind cropping might be in place)
IntPair pointerInDepth = *pPointer;
pointerInDepth.X -= pDepthMD->getCropOriginX();
pointerInDepth.Y -= pDepthMD->getCropOriginY();
if (pointerInDepth.X < (int)pDepthMD->getWidth() &&
pointerInDepth.X >= 0 &&
pointerInDepth.Y < (int)pDepthMD->getHeight() &&
pointerInDepth.Y >= 0)
{
nPointerValue = ((openni::DepthPixel*)(pDepthMD->getData()))[pointerInDepth.Y*pDepthMD->getWidth()+pointerInDepth.X];
glBegin(GL_POINTS);
glColor3f(1,0,0);
glVertex3f(10 + ((nPointerValue/10)*2), WIN_SIZE_Y - 70, 1);
glEnd();
}
}
// Print the scale texts
for (int i=0; i<g_fMaxDepth/10; i+=25)
{
int xPos = i*2 + 10;
// draw a small line in this position
glBegin(GL_LINES);
glColor3f(0, 1, 0);
glVertex2i(xPos, WIN_SIZE_Y - 54);
glVertex2i(xPos, WIN_SIZE_Y - 62);
glEnd();
// place a label under, and in the middle of, that line.
XnUInt32 chars;
xnOSStrFormat(buf, sizeof(buf), &chars, "%d", i);
glColor3f(1,0,0);
glRasterPos2i(xPos - chars*nCharWidth/2, WIN_SIZE_Y - 40);
glPrintString(GLUT_BITMAP_HELVETICA_18,buf);
}
xnOSStrFormat(buf, sizeof(buf), &chars, "%s - Frame %4u, Timestamp %.3f", "Depth"/*getDepthGenerator()->GetInfo().GetInstanceName()*/, pDepthMD->getFrameIndex(), (double)pDepthMD->getTimestamp()/dTimestampDivider);
}
openni::VideoFrameRef* pImageMD = &getColorFrame();
if (pImageMD->isValid())
{
if (buf[0] != '\0')
{
xnOSStrAppend(buf, " | ", sizeof(buf));
}
xnOSStrFormat(buf + strlen(buf), (XnUInt32)(sizeof(buf) - strlen(buf)), &chars, "%s - Frame %4u, Timestamp %.3f", "Color", pImageMD->getFrameIndex(), (double)pImageMD->getTimestamp()/dTimestampDivider);
}
openni::VideoFrameRef* pIRMD = &getIRFrame();
if (pIRMD->isValid())
{
if (buf[0] != '\0')
{
xnOSStrAppend(buf, " | ", sizeof(buf));
}
xnOSStrFormat(buf + strlen(buf), (XnUInt32)(sizeof(buf) - strlen(buf)), &chars, "%s - Frame %4u, Timestamp %.3f", "IR", pIRMD->getFrameIndex(), (double)pIRMD->getTimestamp()/dTimestampDivider);
}
int nYLocation = WIN_SIZE_Y - 88;
glColor3f(1,0,0);
glRasterPos2i(10,nYLocation);
glPrintString(GLUT_BITMAP_HELVETICA_18, buf);
nYLocation -= 26;
if (pPointer != NULL)
{
// Print the pointer text
XnUInt64 nCutOffMin = 0;
XnUInt64 nCutOffMax = (pDepthMD != NULL) ? g_fMaxDepth : 0;
XnChar sPointerValue[100];
if (nPointerValue != g_fMaxDepth)
{
xnOSStrFormat(sPointerValue, sizeof(sPointerValue), &chars, "%.1f", (float)nPointerValue/10);
}
else
{
xnOSStrFormat(sPointerValue, sizeof(sPointerValue), &chars, "-");
}
xnOSStrFormat(buf, sizeof(buf), &chars, "Pointer Value: %s (X:%d Y:%d) Cutoff: %llu-%llu.",
sPointerValue, pPointer->X, pPointer->Y, nCutOffMin, nCutOffMax);
glRasterPos2i(10,nYLocation);
glPrintString(GLUT_BITMAP_HELVETICA_18, buf);
nYLocation -= 26;
}
}
void drawDepth(IntRect* pLocation, IntPair* pPointer)
{
if (g_DrawConfig.Streams.Depth.Coloring != DEPTH_OFF)
{
if (!isDepthOn())
{
drawClosedStream(pLocation, "Depth");
return;
}
openni::VideoFrameRef* pDepthMD = &getDepthFrame();
if (!pDepthMD->isValid())
return;
const openni::DepthPixel* pDepth = (openni::DepthPixel*)pDepthMD->getData();
XN_ASSERT(pDepth);
int width = pDepthMD->getWidth();
int height = pDepthMD->getHeight();
int originX = pDepthMD->getCropOriginX();
int originY = pDepthMD->getCropOriginY();
if (pDepthMD->getFrameIndex() == 0)
{
return;
}
// copy depth into texture-map
for (XnUInt16 nY = originY; nY < height + originY; nY++)
{
XnUInt8* pTexture = TextureMapGetLine(&g_texDepth, nY) + originX*4;
for (XnUInt16 nX = 0; nX < width; nX++, pDepth++, pTexture+=4)
{
XnUInt8 nRed = 0;
XnUInt8 nGreen = 0;
XnUInt8 nBlue = 0;
XnUInt8 nAlpha = g_DrawConfig.Streams.Depth.fTransparency*255;
XnUInt16 nColIndex;
switch (g_DrawConfig.Streams.Depth.Coloring)
{
case LINEAR_HISTOGRAM:
nRed = nGreen = g_pDepthHist[*pDepth]*255;
break;
case PSYCHEDELIC_SHADES:
nAlpha *= (((XnFloat)(*pDepth % 10) / 20) + 0.5);
case PSYCHEDELIC:
switch ((*pDepth/10) % 10)
{
case 0:
nRed = 255;
break;
case 1:
nGreen = 255;
break;
case 2:
nBlue = 255;
break;
case 3:
nRed = 255;
nGreen = 255;
break;
case 4:
nGreen = 255;
nBlue = 255;
break;
case 5:
nRed = 255;
nBlue = 255;
break;
case 6:
nRed = 255;
nGreen = 255;
nBlue = 255;
break;
case 7:
nRed = 127;
nBlue = 255;
break;
case 8:
nRed = 255;
nBlue = 127;
break;
case 9:
nRed = 127;
nGreen = 255;
break;
}
break;
case RAINBOW:
nColIndex = (XnUInt16)((*pDepth / (g_fMaxDepth / 256)));
nRed = PalletIntsR[nColIndex];
nGreen = PalletIntsG[nColIndex];
nBlue = PalletIntsB[nColIndex];
break;
case CYCLIC_RAINBOW:
nColIndex = (*pDepth % 256);
nRed = PalletIntsR[nColIndex];
nGreen = PalletIntsG[nColIndex];
nBlue = PalletIntsB[nColIndex];
break;
case CYCLIC_RAINBOW_HISTOGRAM:
{
float fHist = g_pDepthHist[*pDepth];
nColIndex = (*pDepth % 256);
nRed = PalletIntsR[nColIndex] * fHist;
nGreen = PalletIntsG[nColIndex] * fHist;
nBlue = PalletIntsB[nColIndex] * fHist;
break;
}
default:
assert(0);
return;
}
pTexture[0] = nRed;
pTexture[1] = nGreen;
pTexture[2] = nBlue;
if (*pDepth == 0)
pTexture[3] = 0;
else
pTexture[3] = nAlpha;
}
}
if (pPointer != NULL)
{
TextureMapDrawCursor(&g_texDepth, *pPointer);
}
TextureMapUpdate(&g_texDepth);
TextureMapDraw(&g_texDepth, pLocation);
}
}
