void xnLogGetMasksString(XnChar* csString)
{
switch (g_logData.m_nLogFilteringType)
{
case XN_LOG_WRITE_NONE:
xnOSStrCopy(csString, "NONE", XN_LOG_MASKS_STRING_LEN);
return;
case XN_LOG_WRITE_ALL:
xnOSStrCopy(csString, "ALL", XN_LOG_MASKS_STRING_LEN);
return;
case XN_LOG_WRITE_MASKS:
{
csString[0] = '\0';
for (XnStringsHash::Iterator it = g_logData.m_LogMasks.begin(); it != g_logData.m_LogMasks.end(); ++it)
{
xnOSStrAppend(csString, it.Key(), XN_LOG_MASKS_STRING_LEN);
xnOSStrAppend(csString, ";", XN_LOG_MASKS_STRING_LEN);
}
return;
}
default:
xnOSStrCopy(csString, "UNKNOWN", XN_LOG_MASKS_STRING_LEN);
return;
}
}
XN_C_API XnStatus xnEnumerationErrorsToString(const XnEnumerationErrors* pErrors, XnChar* csBuffer, XnUInt32 nSize)
{
XnStatus nRetVal = XN_STATUS_OK;
XnUInt32 nWritten = 0;
csBuffer[0] = '\0';
nRetVal = xnOSStrAppend(csBuffer, "One or more of the following nodes could not be enumerated:\n\n", nSize);
XN_IS_STATUS_OK(nRetVal);
nWritten = xnOSStrLen(csBuffer);
for (XnEnumerationErrorsIterator it = xnEnumerationErrorsGetFirst(pErrors);
xnEnumerationErrorsIteratorIsValid(it);
it = xnEnumerationErrorsGetNext(it))
{
nRetVal = xnProductionNodeDescriptionToString(xnEnumerationErrorsGetCurrentDescription(it), csBuffer + nWritten, nSize - nWritten);
XN_IS_STATUS_OK(nRetVal);
nRetVal = xnOSStrAppend(csBuffer, ": ", nSize);
XN_IS_STATUS_OK(nRetVal);
nRetVal = xnOSStrAppend(csBuffer, xnGetStatusString(xnEnumerationErrorsGetCurrentError(it)), nSize);
XN_IS_STATUS_OK(nRetVal);
nRetVal = xnOSStrAppend(csBuffer, "\n", nSize);
XN_IS_STATUS_OK(nRetVal);
nWritten = xnOSStrLen(csBuffer);
}
return (XN_STATUS_OK);
}
XnStatus UserSelector::TranslateStateToLabel(const UserSelectionState* pUserState, char *strLabel, XnUInt32 maxStrLen)
{
switch(pUserState->m_eState)
{
case XN_SELECTION_UNSELECTED:
xnOSStrCopy(strLabel,"Not selected",maxStrLen);
return XN_STATUS_OK;
case XN_SELECTION_SELECTED:
xnOSStrCopy(strLabel,"Selected [",maxStrLen);
xnOSStrAppend(strLabel,GetCalibrationErrorString((XnCalibrationStatus)pUserState->m_subState),maxStrLen);
xnOSStrAppend(strLabel,"]",maxStrLen);
return XN_STATUS_OK;
case XN_SELECTION_TRACKING:
xnOSStrCopy(strLabel,"Tracking",maxStrLen);
return XN_STATUS_OK;
case XN_SELECTION_FAILED:
xnOSStrCopy(strLabel,"Failed to track [",maxStrLen);
xnOSStrAppend(strLabel,GetCalibrationErrorString((XnCalibrationStatus)pUserState->m_subState),maxStrLen);
xnOSStrAppend(strLabel,"]",maxStrLen);
return XN_STATUS_OK;
default:
xnOSStrCopy(strLabel,"User in illegal state!\n",maxStrLen);
return XN_STATUS_ERROR;
}
}
XN_C_API XnStatus xnLogSetOutputFolder(const XnChar* strOutputFolder)
{
XnStatus nRetVal = XN_STATUS_OK;
// check if folder exists
XnBool bDirExists = FALSE;
nRetVal = xnOSDoesDirecotyExist(strOutputFolder, &bDirExists);
XN_IS_STATUS_OK(nRetVal);
if (!bDirExists)
{
// create it
nRetVal = xnOSCreateDirectory(strOutputFolder);
XN_IS_STATUS_OK(nRetVal);
}
// place it in a temp buffer, just to make sure everything succeeds
XnChar strDirName[XN_FILE_MAX_PATH];
nRetVal = xnOSGetFullPathName(strOutputFolder, strDirName, XN_FILE_MAX_PATH);
XN_IS_STATUS_OK(nRetVal);
nRetVal = xnOSStrAppend(strDirName, XN_FILE_DIR_SEP, XN_FILE_MAX_PATH);
XN_IS_STATUS_OK(nRetVal);
// OK. replace
xnOSStrCopy(g_logData.m_strLogDir, strDirName, XN_FILE_MAX_PATH);
// restart file writer
xnLogStartNewFile();
return (XN_STATUS_OK);
}
//---------------------------------------------------------------------------
// Code
//---------------------------------------------------------------------------
XnStatus resolveLicensesFile(XnChar* strFileName, XnUInt32 nBufSize)
{
XnStatus nRetVal = XN_STATUS_OK;
nRetVal = xnGetOpenNIConfFilesPath(strFileName, nBufSize);
XN_IS_STATUS_OK(nRetVal);
nRetVal = xnOSStrAppend(strFileName, "licenses.xml", nBufSize);
XN_IS_STATUS_OK(nRetVal);
return (XN_STATUS_OK);
}
XN_C_API XnStatus xnOSGetFileName(const XnChar* cpFilePath, XnChar* cpFileName, const XnUInt32 nBufferSize)
{
XnChar ext[XN_FILE_MAX_PATH];
XN_VALIDATE_INPUT_PTR(cpFilePath);
XN_VALIDATE_OUTPUT_PTR(cpFileName);
errno_t err = _splitpath_s(cpFilePath, NULL, 0, NULL, 0, cpFileName, nBufferSize, ext, sizeof(ext));
if (err == ERANGE)
{
return XN_STATUS_OUTPUT_BUFFER_OVERFLOW;
}
else if (err != 0)
{
return XN_STATUS_ERROR;
}
XnStatus nRetVal = xnOSStrAppend(cpFileName, ext, nBufferSize);
XN_IS_STATUS_OK(nRetVal);
return XN_STATUS_OK;
}
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;
}
}
XnUInt64 XnDataProcessor::GetTimeStamp(XnUInt32 nDeviceTimeStamp)
{
const XnUInt64 nWrapPoint = ((XnUInt64)XN_MAX_UINT32) + 1;
XnUInt64 nResultInTicks;
XnUInt64 nNow;
xnOSGetHighResTimeStamp(&nNow);
const XnUInt32 nDumpCommentMaxLength = 200;
XnChar csDumpComment[nDumpCommentMaxLength] = "";
XnBool bCheckSanity = TRUE;
// we register the first TS calculated as time-zero. Every stream's TS data will be
// synchronized with it
if (m_pDevicePrivateData->nGlobalReferenceTS == 0)
{
xnOSEnterCriticalSection(&m_pDevicePrivateData->hEndPointsCS);
if (m_pDevicePrivateData->nGlobalReferenceTS == 0)
{
m_pDevicePrivateData->nGlobalReferenceTS = nDeviceTimeStamp;
m_pDevicePrivateData->nGlobalReferenceOSTime = nNow;
}
xnOSLeaveCriticalSection(&m_pDevicePrivateData->hEndPointsCS);
}
if (m_TimeStampData.bFirst)
{
/*
This is a bit tricky, as we need to synchronize the first timestamp of different streams.
We somehow need to translate 32-bit tick counts to 64-bit timestamps. The device timestamps
wrap-around every ~71.5 seconds (for PS1080 @ 60 MHz).
Lets assume the first packet of the first stream got timestamp X. Now we get the first packet of another
stream with a timestamp Y.
We need to figure out what is the relation between X and Y.
We do that by analyzing the following scenarios:
1. Y is after X, in the same period (no wraparound yet).
2. Y is after X, in a different period (one or more wraparounds occurred).
3. Y is before X, in the same period (might happen due to race condition).
4. Y is before X, in a different period (this can happen if X is really small, and Y is almost at wraparound).
The following code tried to handle all those cases. It uses an OS timer to try and figure out how
many wraparounds occurred.
*/
// estimate the number of wraparound that occurred using OS time
XnUInt64 nOSTime = nNow - m_pDevicePrivateData->nGlobalReferenceOSTime;
// calculate wraparound length
XnDouble fWrapAroundInMicroseconds = nWrapPoint / (XnDouble)m_pDevicePrivateData->fDeviceFrequency;
// perform a rough estimation
XnInt32 nWraps = (XnInt32)(nOSTime / fWrapAroundInMicroseconds);
// now fix the estimation by clipping TS to the correct wraparounds
XnInt64 nEstimatedTicks =
nWraps * nWrapPoint + // wraps time
nDeviceTimeStamp - m_pDevicePrivateData->nGlobalReferenceTS;
XnInt64 nEstimatedTime = (XnInt64)(nEstimatedTicks / (XnDouble)m_pDevicePrivateData->fDeviceFrequency);
if (nEstimatedTime < nOSTime - 0.5 * fWrapAroundInMicroseconds)
nWraps++;
else if (nEstimatedTime > nOSTime + 0.5 * fWrapAroundInMicroseconds)
nWraps--;
// handle the two special cases - 3 & 4 in which we get a timestamp which is
// *before* global TS (meaning before time 0)
if (nWraps < 0 || // case 4
(nWraps == 0 && nDeviceTimeStamp < m_pDevicePrivateData->nGlobalReferenceTS)) // case 3
{
nDeviceTimeStamp = m_pDevicePrivateData->nGlobalReferenceTS;
nWraps = 0;
}
m_TimeStampData.nReferenceTS = m_pDevicePrivateData->nGlobalReferenceTS;
m_TimeStampData.nTotalTicksAtReferenceTS = nWrapPoint * nWraps;
m_TimeStampData.nLastDeviceTS = 0;
m_TimeStampData.bFirst = FALSE;
nResultInTicks = 0;
bCheckSanity = FALSE; // no need.
sprintf(csDumpComment, "Init. Total Ticks in Ref TS: %llu", m_TimeStampData.nTotalTicksAtReferenceTS);
}
if (nDeviceTimeStamp > m_TimeStampData.nLastDeviceTS) // this is the normal case
{
nResultInTicks = m_TimeStampData.nTotalTicksAtReferenceTS + nDeviceTimeStamp - m_TimeStampData.nReferenceTS;
}
else // wrap around occurred
{
// add the passed time to the reference time
m_TimeStampData.nTotalTicksAtReferenceTS += (nWrapPoint + nDeviceTimeStamp - m_TimeStampData.nReferenceTS);
// mark reference timestamp
m_TimeStampData.nReferenceTS = nDeviceTimeStamp;
sprintf(csDumpComment, "Wrap around. Refernce TS: %u / TotalTicksAtReference: %llu", m_TimeStampData.nReferenceTS, m_TimeStampData.nTotalTicksAtReferenceTS);
nResultInTicks = m_TimeStampData.nTotalTicksAtReferenceTS;
}
m_TimeStampData.nLastDeviceTS = nDeviceTimeStamp;
// calculate result in microseconds
// NOTE: Intel compiler does too much optimization, and we loose up to 5 milliseconds. We perform
// the entire calculation in XnDouble as a workaround
XnDouble dResultTimeMicroSeconds = (XnDouble)nResultInTicks / (XnDouble)m_pDevicePrivateData->fDeviceFrequency;
XnUInt64 nResultTimeMilliSeconds = (XnUInt64)(dResultTimeMicroSeconds / 1000.0);
XnBool bIsSane = TRUE;
// perform sanity check
if (bCheckSanity && (nResultTimeMilliSeconds > (m_TimeStampData.nLastResultTime + XN_SENSOR_TIMESTAMP_SANITY_DIFF*1000)))
{
bIsSane = FALSE;
xnOSStrAppend(csDumpComment, ",Didn't pass sanity. Will try to re-sync.", nDumpCommentMaxLength);
}
// calc result
XnUInt64 nResult = (m_pDevicePrivateData->pSensor->IsHighResTimestamps() ? (XnUInt64)dResultTimeMicroSeconds : nResultTimeMilliSeconds);
// dump it
xnDumpFileWriteString(m_pDevicePrivateData->TimestampsDump, "%llu,%s,%u,%llu,%s\n", nNow, m_TimeStampData.csStreamName, nDeviceTimeStamp, nResult, csDumpComment);
if (bIsSane)
{
m_TimeStampData.nLastResultTime = nResultTimeMilliSeconds;
return (nResult);
}
else
{
// sanity failed. We lost sync. restart
m_TimeStampData.bFirst = TRUE;
return GetTimeStamp(nDeviceTimeStamp);
}
}
