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); } }