void statisticsAddFrame()
{
if (g_StatisticsState == SHOULD_COLLECT)
{
g_StatisticsState = COLLECTING;
}
if (g_StatisticsState == COLLECTING)
{
const DepthMetaData* pDepthMD = getDepthMetaData();
const XnDepthPixel* pDepth = pDepthMD->Data();
for (XnUInt32 y = pDepthMD->YOffset(); y < pDepthMD->YRes() + pDepthMD->YOffset(); ++y)
{
XnPixelStatistics* pPixel = &g_PixelStatistics[y*pDepthMD->FullXRes() + pDepthMD->XOffset()];
for (XnUInt32 x = pDepthMD->XOffset(); x < pDepthMD->XRes() + pDepthMD->XOffset(); ++x, ++pDepth, ++pPixel)
{
XnDepthPixel nDepth = *pDepth;
if (nDepth > pPixel->nMax)
pPixel->nMax = nDepth;
if (nDepth < pPixel->nMin)
pPixel->nMin = nDepth;
pPixel->nSum += nDepth;
pPixel->nSumSquare += (nDepth * nDepth);
pPixel->nCount++;
pPixel->dAverage = pPixel->nSum / pPixel->nCount;
pPixel->dStdDev = sqrt( (1.0/pPixel->nCount) * (pPixel->nSumSquare) - (pPixel->dAverage * pPixel->dAverage) );
}
}
}
}
void captureSingleFrame(int)
{
int num = findUniqueFileName();
XnChar csImageFileName[XN_FILE_MAX_PATH];
XnChar csDepthFileName[XN_FILE_MAX_PATH];
XnChar csIRFileName[XN_FILE_MAX_PATH];
getImageFileName(num, csImageFileName);
getDepthFileName(num, csDepthFileName);
getIRFileName(num, csIRFileName);
const ImageMetaData* pImageMD = getImageMetaData();
if (pImageMD != NULL)
{
xnOSSaveFile(csImageFileName, pImageMD->Data(), pImageMD->DataSize());
}
const IRMetaData* pIRMD = getIRMetaData();
if (pIRMD != NULL)
{
xnOSSaveFile(csIRFileName, pIRMD->Data(), pIRMD->DataSize());
}
const DepthMetaData* pDepthMD = getDepthMetaData();
if (pDepthMD != NULL)
{
xnOSSaveFile(csDepthFileName, pDepthMD->Data(), pDepthMD->DataSize());
}
g_Capture.nCapturedFrameUniqueID = num + 1;
// displayMessage("Frames saved with ID %d", num);
printf("Frames saved with ID %d\n", num);
}
// --------------------------------
// Code
// --------------------------------
void statisticsInit()
{
g_PixelStatistics = NULL;
g_StatisticsState = NOT_COLLECTING;
const DepthMetaData* pDepthMD = getDepthMetaData();
if (pDepthMD != NULL)
{
g_PixelStatistics = new XnPixelStatistics[pDepthMD->FullXRes() * pDepthMD->FullYRes()];
}
}
void getStatisticsMessage(char* csMessage)
{
switch (g_StatisticsState)
{
case NOT_COLLECTING:
case SHOULD_COLLECT:
csMessage[0] = 0;
break;
case COLLECTING:
case COLLECTION_ENDED:
const DepthMetaData* pDepthMD = getDepthMetaData();
sprintf(csMessage, "Collected Statistics for %u frames", g_PixelStatistics[pDepthMD->YOffset()*pDepthMD->FullXRes() + pDepthMD->XOffset()].nCount);
break;
}
}
void statisticsStart(int)
{
const DepthMetaData* pDepthMD = getDepthMetaData();
if (pDepthMD == NULL)
{
displayMessage("Can't collect statistics: Depth is turned off!");
return;
}
int nCount = pDepthMD->FullXRes() * pDepthMD->FullYRes();
XnDepthPixel nMaxDepth = getDepthGenerator()->GetDeviceMaxDepth();
XnPixelStatistics* pPixel = g_PixelStatistics;
for (int i = 0; i < nCount; ++i, ++pPixel)
{
xnOSMemSet(pPixel, 0, sizeof(XnPixelStatistics));
pPixel->nMin = nMaxDepth;
}
g_StatisticsState = SHOULD_COLLECT;
}
void drawDepth(IntRect* pLocation, IntPair* pPointer)
{
if (g_DrawConfig.Streams.Depth.Coloring != DEPTH_OFF)
{
if (!isDepthOn())
{
drawClosedStream(pLocation, "Depth");
return;
}
const DepthMetaData* pDepthMD = getDepthMetaData();
const XnDepthPixel* pDepth = pDepthMD->Data();
XN_ASSERT(pDepth);
if (pDepthMD->FrameID() == 0)
{
return;
}
if (g_DrawConfig.Streams.Depth.Coloring == STANDARD_DEVIATION)
{
XnPixelStatistics* pStatistics = g_PixelStatistics;
for (XnUInt16 nY = pDepthMD->YOffset(); nY < pDepthMD->YRes() + pDepthMD->YOffset(); nY++)
{
XnUInt8* pTexture = TextureMapGetLine(&g_texDepth, nY) + pDepthMD->XOffset()*4;
for (XnUInt16 nX = 0; nX < pDepthMD->XRes(); nX++, pTexture+=4, pStatistics++)
{
pTexture[0] = pTexture[1] = XN_MIN((int)pStatistics->dStdDev, 255);
pTexture[2] = 0;
pTexture[3] = g_DrawConfig.Streams.Depth.fTransparency*255;
}
}
}
else
{
// copy depth into texture-map
for (XnUInt16 nY = pDepthMD->YOffset(); nY < pDepthMD->YRes() + pDepthMD->YOffset(); nY++)
{
XnUInt8* pTexture = TextureMapGetLine(&g_texDepth, nY) + pDepthMD->XOffset()*4;
for (XnUInt16 nX = 0; nX < pDepthMD->XRes(); 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_nMaxDepth / 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;
}
pTexture[0] = nRed;
pTexture[1] = nGreen;
pTexture[2] = nBlue;
if (*pDepth == 0)
pTexture[3] = 0;
else
pTexture[3] = nAlpha;
}
}
} // not STANDRARD_DEVIATION
if (pPointer != NULL)
{
TextureMapDrawCursor(&g_texDepth, *pPointer);
}
TextureMapUpdate(&g_texDepth);
TextureMapDraw(&g_texDepth, pLocation);
}
}
void drawColorImage(IntRect* pLocation, IntPair* pPointer, int pointerRed, int pointerGreen, int pointerBlue)
{
if (g_DrawConfig.Streams.bBackground)
TextureMapDraw(&g_texBackground, pLocation);
if (g_DrawConfig.Streams.Image.Coloring == IMAGE_OFF)
return;
if (!isImageOn() && !isIROn())
{
drawClosedStream(pLocation, "Image");
return;
}
const MapMetaData* pImageMD;
const XnUInt8* pImage = NULL;
if (isImageOn())
{
pImageMD = getImageMetaData();
pImage = getImageMetaData()->Data();
}
else if (isIROn())
{
pImageMD = getIRMetaData();
pImage = (const XnUInt8*)getIRMetaData()->Data();
}
else
return;
if (pImageMD->FrameID() == 0)
{
return;
}
const DepthMetaData* pDepthMetaData = getDepthMetaData();
double grayscale16Factor = 1.0;
if (pImageMD->PixelFormat() == XN_PIXEL_FORMAT_GRAYSCALE_16_BIT)
{
int nPixelsCount = pImageMD->XRes()*pImageMD->YRes();
XnUInt16* pPixel = (XnUInt16*)pImage;
for (int i = 0; i < nPixelsCount; ++i,++pPixel)
{
if (*pPixel > g_nMaxGrayscale16Value)
g_nMaxGrayscale16Value = *pPixel;
}
if (g_nMaxGrayscale16Value > 0)
{
grayscale16Factor = 255.0 / g_nMaxGrayscale16Value;
}
}
for (XnUInt16 nY = pImageMD->YOffset(); nY < pImageMD->YRes() + pImageMD->YOffset(); nY++)
{
XnUInt8* pTexture = TextureMapGetLine(&g_texImage, nY) + pImageMD->XOffset()*4;
if (pImageMD->PixelFormat() == XN_PIXEL_FORMAT_YUV422)
{
YUV422ToRGB888(pImage, pTexture, pImageMD->XRes()*2, g_texImage.Size.X*g_texImage.nBytesPerPixel);
pImage += pImageMD->XRes()*2;
}
else
{
for (XnUInt16 nX = 0; nX < pImageMD->XRes(); nX++, pTexture+=4)
{
XnInt32 nDepthIndex = 0;
if (pDepthMetaData != NULL)
{
XnDouble dRealX = (nX + pImageMD->XOffset()) / (XnDouble)pImageMD->FullXRes();
XnDouble dRealY = nY / (XnDouble)pImageMD->FullYRes();
XnUInt32 nDepthX = dRealX * pDepthMetaData->FullXRes() - pDepthMetaData->XOffset();
XnUInt32 nDepthY = dRealY * pDepthMetaData->FullYRes() - pDepthMetaData->YOffset();
if (nDepthX >= pDepthMetaData->XRes() || nDepthY >= pDepthMetaData->YRes())
{
nDepthIndex = -1;
}
else
{
nDepthIndex = nDepthY*pDepthMetaData->XRes() + nDepthX;
}
}
switch (pImageMD->PixelFormat())
{
case XN_PIXEL_FORMAT_RGB24:
pTexture[0] = pImage[0];
pTexture[1] = pImage[1];
pTexture[2] = pImage[2];
pImage+=3;
break;
case XN_PIXEL_FORMAT_GRAYSCALE_8_BIT:
pTexture[0] = pTexture[1] = pTexture[2] = *pImage;
pImage+=1;
break;
case XN_PIXEL_FORMAT_GRAYSCALE_16_BIT:
XnUInt16* p16 = (XnUInt16*)pImage;
XnUInt8 textureValue = 0;
textureValue = (XnUInt8)((*p16) * grayscale16Factor);
pTexture[0] = pTexture[1] = pTexture[2] = textureValue;
pImage+=2;
break;
}
// decide if pixel should be lit or not
if (g_DrawConfig.Streams.Image.Coloring == DEPTH_MASKED_IMAGE &&
(pDepthMetaData == NULL || nDepthIndex == -1 || pDepthMetaData->Data()[nDepthIndex] == 0))
{
pTexture[3] = 0;
}
else
{
pTexture[3] = 255;
}
}
}
}
if (pPointer != NULL)
{
TextureMapDrawCursor(&g_texImage, *pPointer, pointerRed, pointerGreen, pointerBlue);
}
TextureMapUpdate(&g_texImage);
TextureMapDraw(&g_texImage, 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.ImageLocation.uBottom = 0;
g_DrawConfig.ImageLocation.uTop = WIN_SIZE_Y - 1;
g_DrawConfig.ImageLocation.uLeft = 0;
g_DrawConfig.ImageLocation.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.ImageLocation.uTop = WIN_SIZE_Y / 2 - 1;
g_DrawConfig.ImageLocation.uLeft = WIN_SIZE_X / 2;
}
// Texture map init
const DepthMetaData* pDepthMD = getDepthMetaData();
if (isDepthOn())
{
g_nMaxDepth = getDepthGenerator()->GetDeviceMaxDepth();
TextureMapInit(&g_texDepth, pDepthMD->FullXRes(), pDepthMD->FullYRes(), 4, pDepthMD->XRes(), pDepthMD->YRes());
fixLocation(&g_DrawConfig.DepthLocation, pDepthMD->FullXRes(), pDepthMD->FullYRes());
}
const MapMetaData* pImageMD = NULL;
if (isImageOn())
{
pImageMD = getImageMetaData();
}
else if (isIROn())
{
pImageMD = getIRMetaData();
}
if (pImageMD != NULL)
{
TextureMapInit(&g_texImage, pImageMD->FullXRes(), pImageMD->FullYRes(), 4, pImageMD->XRes(), pImageMD->YRes());
fixLocation(&g_DrawConfig.ImageLocation, pImageMD->FullXRes(), pImageMD->FullYRes());
}
// check if pointer is over a map
bool bOverDepth = (pDepthMD != NULL) && isPointInRect(g_DrawUserInput.Cursor, &g_DrawConfig.DepthLocation);
bool bOverImage = (pImageMD != NULL) && isPointInRect(g_DrawUserInput.Cursor, &g_DrawConfig.ImageLocation);
bool bDrawDepthPointer = false;
bool bDrawImagePointer = false;
int imagePointerRed = 255;
int imagePointerGreen = 0;
int imagePointerBlue = 0;
IntPair pointerInDepth;
IntPair pointerInImage;
if (bOverImage)
{
pointerInImage.X = (double)(g_DrawUserInput.Cursor.X - g_DrawConfig.ImageLocation.uLeft) / (g_DrawConfig.ImageLocation.uRight - g_DrawConfig.ImageLocation.uLeft + 1) * pImageMD->FullXRes();
pointerInImage.Y = (double)(g_DrawUserInput.Cursor.Y - g_DrawConfig.ImageLocation.uBottom) / (g_DrawConfig.ImageLocation.uTop - g_DrawConfig.ImageLocation.uBottom + 1) * pImageMD->FullYRes();
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->FullXRes();
pointerInDepth.Y = (double)(g_DrawUserInput.Cursor.Y - g_DrawConfig.DepthLocation.uBottom) / (g_DrawConfig.DepthLocation.uTop - g_DrawConfig.DepthLocation.uBottom + 1) * pDepthMD->FullYRes();
// make sure we're in cropped area
if (pointerInDepth.X >= pDepthMD->XOffset() && pointerInDepth.X < (pDepthMD->XOffset() + pDepthMD->XRes()) &&
pointerInDepth.Y >= pDepthMD->YOffset() && pointerInDepth.Y < (pDepthMD->YOffset() + pDepthMD->YRes()))
{
bDrawDepthPointer = true;
if (!bOverImage && g_DrawConfig.bShowPointer)
{
// try to translate depth pixel to image
if (getImageCoordinatesForDepthPixel(pointerInDepth.X, pointerInDepth.Y, pointerInImage.X, pointerInImage.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();
drawColorImage(&g_DrawConfig.ImageLocation, bDrawImagePointer ? &pointerInImage : NULL, imagePointerRed, imagePointerGreen, imagePointerBlue);
drawDepth(&g_DrawConfig.DepthLocation, bDrawDepthPointer ? &pointerInDepth : NULL);
printStatisticsInfo();
printRecordingInfo();
if (g_DrawConfig.bShowPointer)
drawPointerMode(bDrawDepthPointer ? &pointerInDepth : NULL);
drawUserInput(!bOverDepth && !bOverImage);
drawUserMessage();
drawPlaybackSpeed();
if (g_DrawConfig.strErrorState != NULL)
drawErrorState();
if (g_DrawConfig.bHelp)
drawHelpScreen();
glutSwapBuffers();
}
void drawPointerMode(IntPair* pPointer)
{
char buf[512] = "";
int nCharWidth = glutBitmapWidth(GLUT_BITMAP_HELVETICA_18, '0');
int nPointerValue = 0;
XnDouble dTimestampDivider = 1E6;
const DepthMetaData* pDepthMD = getDepthMetaData();
if (pDepthMD != NULL)
{
// 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 < pDepthMD->ZRes(); 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->XOffset();
pointerInDepth.Y -= pDepthMD->YOffset();
if (pointerInDepth.X < (int)pDepthMD->XRes() && pointerInDepth.Y < (int)pDepthMD->YRes())
{
nPointerValue = (*pDepthMD)(pointerInDepth.X, pointerInDepth.Y);
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 < pDepthMD->ZRes()/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.
int chars = sprintf(buf, "%d", i);
glColor3f(1,0,0);
glRasterPos2i(xPos - chars*nCharWidth/2, WIN_SIZE_Y - 40);
glPrintString(GLUT_BITMAP_HELVETICA_18,buf);
}
sprintf(buf, "%s - Frame %4u, Timestamp %.3f", getDepthGenerator()->GetInfo().GetInstanceName(), pDepthMD->FrameID(), (double)pDepthMD->Timestamp()/dTimestampDivider);
}
const ImageMetaData* pImageMD = getImageMetaData();
if (pImageMD != NULL)
{
if (buf[0] != '\0')
sprintf(buf + strlen(buf), " | ");
sprintf(buf + strlen(buf), "%s - Frame %4u, Timestamp %.3f", getImageGenerator()->GetInfo().GetInstanceName(), pImageMD->FrameID(), (double)pImageMD->Timestamp()/dTimestampDivider);
}
const IRMetaData* pIRMD = getIRMetaData();
if (pIRMD != NULL)
{
if (buf[0] != '\0')
sprintf(buf + strlen(buf), " | ");
sprintf(buf + strlen(buf), "%s - Frame %4u, Timestamp %.3f", getIRGenerator()->GetInfo().GetInstanceName(), pIRMD->FrameID(), (double)pIRMD->Timestamp()/dTimestampDivider);
}
const AudioMetaData* pAudioMD = getAudioMetaData();
if (pAudioMD != NULL)
{
if (buf[0] != '\0')
sprintf(buf + strlen(buf), " | ");
sprintf(buf + strlen(buf), "%s - Timestamp %.3f", getAudioGenerator()->GetInfo().GetInstanceName(), (double)pAudioMD->Timestamp()/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 && isStatisticsActive())
{
XnPixelStatistics* pStatistics = &g_PixelStatistics[pPointer->Y * pDepthMD->XRes() + pPointer->X];
sprintf(buf, "Collected: %3u, Min: %4u Max: %4u Avg: %6.2f StdDev: %6.2f",
pStatistics->nCount, pStatistics->nMin, pStatistics->nMax, pStatistics->dAverage, pStatistics->dStdDev);
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_nMaxDepth : 0;
XnChar sPointerValue[100];
if (nPointerValue != g_nMaxDepth)
{
sprintf(sPointerValue, "%.1f", (float)nPointerValue/10);
}
else
{
sprintf(sPointerValue, "-");
}
sprintf(buf, "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;
}
}