void XnFrameStreamProcessor::ProcessPacketChunk(const XnSensorProtocolResponseHeader* pHeader, const XnUChar* pData, XnUInt32 nDataOffset, XnUInt32 nDataSize) { XN_PROFILING_START_SECTION("XnFrameStreamProcessor::ProcessPacketChunk"); // if first data from SOF packet if (pHeader->nType == m_nTypeSOF && nDataOffset == 0) { if (!m_bAllowDoubleSOF || pHeader->nPacketID != (m_nLastSOFPacketID + 1)) { m_nLastSOFPacketID = pHeader->nPacketID; OnStartOfFrame(pHeader); } } if (!m_bFrameCorrupted) { xnDumpWriteBuffer(m_InDump, pData, nDataSize); ProcessFramePacketChunk(pHeader, pData, nDataOffset, nDataSize); } // if last data from EOF packet if (pHeader->nType == m_nTypeEOF && (nDataOffset + nDataSize) == pHeader->nBufSize) { OnEndOfFrame(pHeader); } XN_PROFILING_END_SECTION }
void XnDataProcessor::ProcessData(const XnSensorProtocolResponseHeader* pHeader, const XnUChar* pData, XnUInt32 nDataOffset, XnUInt32 nDataSize) { XN_PROFILING_START_SECTION("XnDataProcessor::ProcessData") // count these bytes m_nBytesReceived += nDataSize; // check if we start a new packet if (nDataOffset == 0) { // make sure no packet was lost if (pHeader->nPacketID != m_nLastPacketID+1 && pHeader->nPacketID != 0) { xnLogWarning(XN_MASK_SENSOR_PROTOCOL, "%s: Expected %x, got %x", m_csName, m_nLastPacketID+1, pHeader->nPacketID); OnPacketLost(); } m_nLastPacketID = pHeader->nPacketID; // log packet arrival XnUInt64 nNow; xnOSGetHighResTimeStamp(&nNow); xnDumpFileWriteString(m_pDevicePrivateData->MiniPacketsDump, "%llu,0x%hx,0x%hx,0x%hx,%u\n", nNow, pHeader->nType, pHeader->nPacketID, pHeader->nBufSize, pHeader->nTimeStamp); } ProcessPacketChunk(pHeader, pData, nDataOffset, nDataSize); XN_PROFILING_END_SECTION }
void XnUncompressedDepthProcessor::ProcessFramePacketChunk(const XnSensorProtocolResponseHeader* pHeader, const XnUChar* pData, XnUInt32 nDataOffset, XnUInt32 nDataSize) { XN_PROFILING_START_SECTION("XnUncompressedDepthProcessor::ProcessFramePacketChunk") // when depth is uncompressed, we can just copy it directly to write buffer XnBuffer* pWriteBuffer = GetWriteBuffer(); // make sure we have enough room if (CheckWriteBufferForOverflow(nDataSize)) { // sometimes, when packets are lost, we get uneven number of bytes, so we need to complete // one byte, in order to keep UINT16 alignment if (nDataSize % 2 != 0) { nDataSize--; pData++; } // copy values. Make sure we do not get corrupted shifts XnUInt16* pRaw = (XnUInt16*)(pData); XnUInt16* pRawEnd = (XnUInt16*)(pData + nDataSize); XnDepthPixel* pWriteBuf = (XnDepthPixel*)pWriteBuffer->GetUnsafeWritePointer(); while (pRaw < pRawEnd) { *pWriteBuf = GetOutput(XN_MIN(*pRaw, XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)); ++pRaw; ++pWriteBuf; } pWriteBuffer->UnsafeUpdateSize(nDataSize); } XN_PROFILING_END_SECTION }
void XnUncompressedYUVtoRGBImageProcessor::ProcessFramePacketChunk(const XnSensorProtocolResponseHeader* pHeader, const XnUChar* pData, XnUInt32 nDataOffset, XnUInt32 nDataSize) { XN_PROFILING_START_SECTION("XnUncompressedYUVtoRGBImageProcessor::ProcessFramePacketChunk") XnBuffer* pWriteBuffer = GetWriteBuffer(); if (m_ContinuousBuffer.GetSize() != 0) { // fill in to a whole element XnUInt32 nReadBytes = XN_MIN(nDataSize, XN_YUV_TO_RGB_INPUT_ELEMENT_SIZE - m_ContinuousBuffer.GetSize()); m_ContinuousBuffer.UnsafeWrite(pData, nReadBytes); pData += nReadBytes; nDataSize -= nReadBytes; if (m_ContinuousBuffer.GetSize() == XN_YUV_TO_RGB_INPUT_ELEMENT_SIZE) { if (CheckWriteBufferForOverflow(XN_YUV_TO_RGB_OUTPUT_ELEMENT_SIZE)) { // process it XnUInt32 nActualRead = 0; XnUInt32 nOutputSize = pWriteBuffer->GetFreeSpaceInBuffer(); YUV422ToRGB888(m_ContinuousBuffer.GetData(), pWriteBuffer->GetUnsafeWritePointer(), XN_YUV_TO_RGB_INPUT_ELEMENT_SIZE, &nActualRead, &nOutputSize); pWriteBuffer->UnsafeUpdateSize(XN_YUV_TO_RGB_OUTPUT_ELEMENT_SIZE); } m_ContinuousBuffer.Reset(); } } if (CheckWriteBufferForOverflow(nDataSize / XN_YUV_TO_RGB_INPUT_ELEMENT_SIZE * XN_YUV_TO_RGB_OUTPUT_ELEMENT_SIZE)) { XnUInt32 nActualRead = 0; XnUInt32 nOutputSize = pWriteBuffer->GetFreeSpaceInBuffer(); YUV422ToRGB888(pData, pWriteBuffer->GetUnsafeWritePointer(), nDataSize, &nActualRead, &nOutputSize); pWriteBuffer->UnsafeUpdateSize(nOutputSize); pData += nActualRead; nDataSize -= nActualRead; // if we have any bytes left, store them for next packet. if (nDataSize > 0) { // no need to check for overflow. there can not be a case in which more than XN_INPUT_ELEMENT_SIZE // are left. m_ContinuousBuffer.UnsafeWrite(pData, nDataSize); } } XN_PROFILING_END_SECTION }
void XnPassThroughImageProcessor::ProcessFramePacketChunk(const XnSensorProtocolResponseHeader* /*pHeader*/, const XnUChar* pData, XnUInt32 /*nDataOffset*/, XnUInt32 nDataSize) { XN_PROFILING_START_SECTION("XnUncompressedYUVImageProcessor::ProcessFramePacketChunk") // when image is uncompressed, we can just copy it directly to write buffer XnBuffer* pWriteBuffer = GetWriteBuffer(); // make sure we have enough room if (CheckWriteBufferForOverflow(nDataSize)) { pWriteBuffer->UnsafeWrite(pData, nDataSize); } XN_PROFILING_END_SECTION }
void XnUncompressedBayerProcessor::ProcessFramePacketChunk(const XnSensorProtocolResponseHeader* pHeader, const XnUChar* pData, XnUInt32 nDataOffset, XnUInt32 nDataSize) { XN_PROFILING_START_SECTION("XnUncompressedBayerProcessor::ProcessFramePacketChunk") // if output format is Gray8, we can write directly to output buffer. otherwise, we need // to write to a temp buffer. XnBuffer* pWriteBuffer = (GetStream()->GetOutputFormat() == XN_OUTPUT_FORMAT_GRAYSCALE8) ? GetWriteBuffer() : &m_UncompressedBayerBuffer; // make sure we have enough room if (CheckWriteBufferForOverflow(nDataSize)) { pWriteBuffer->UnsafeWrite(pData, nDataSize); } XN_PROFILING_END_SECTION }
void XnFrameStreamProcessor::ProcessPacketChunk(const XnSensorProtocolResponseHeader* pHeader, const XnUChar* pData, XnUInt32 nDataOffset, XnUInt32 nDataSize) { XN_PROFILING_START_SECTION("XnFrameStreamProcessor::ProcessPacketChunk"); // if first data from SOF packet if (pHeader->nType == m_nTypeSOF && nDataOffset == 0) { if (!m_bAllowDoubleSOF || pHeader->nPacketID != (m_nLastSOFPacketID + 1)) { XnUInt64 currOSTime; xnOSGetTimeStamp(&currOSTime); static const XnUInt32 halfSensorPeriod = 33/2; // in milliseconds if(currOSTime - m_nLastSOFTimestamp > 1000 / XnSensor::ms_SoftVideoMode.fps - halfSensorPeriod) { xnLogVerbose(XN_MASK_SENSOR_PROTOCOL, "%s: Processing frame %d, t = %d (dt = %d)", m_csName, pHeader->nPacketID, int(currOSTime), int(currOSTime - m_nLastSOFTimestamp)); m_nLastSOFPacketID = pHeader->nPacketID; OnStartOfFrame(pHeader); m_nLastSOFTimestamp = currOSTime; m_bProcessNextFrame = TRUE; } else { m_bProcessNextFrame = FALSE; } } } if(!m_bProcessNextFrame) return; if (!m_bFrameCorrupted) { xnDumpFileWriteBuffer(m_InDump, pData, nDataSize); ProcessFramePacketChunk(pHeader, pData, nDataOffset, nDataSize); } // if last data from EOF packet if (pHeader->nType == m_nTypeEOF && (nDataOffset + nDataSize) == pHeader->nBufSize) { OnEndOfFrame(pHeader); } XN_PROFILING_END_SECTION }
void XnPacked11DepthProcessor::ProcessFramePacketChunk(const XnSensorProtocolResponseHeader* /*pHeader*/, const XnUChar* pData, XnUInt32 /*nDataOffset*/, XnUInt32 nDataSize) { XN_PROFILING_START_SECTION("XnPacked11DepthProcessor::ProcessFramePacketChunk") XnStatus nRetVal = XN_STATUS_OK; // check if we have data from previous packet if (m_ContinuousBuffer.GetSize() != 0) { // fill in to a whole element XnUInt32 nReadBytes = XN_MIN(nDataSize, XN_INPUT_ELEMENT_SIZE - m_ContinuousBuffer.GetSize()); m_ContinuousBuffer.UnsafeWrite(pData, nReadBytes); pData += nReadBytes; nDataSize -= nReadBytes; if (m_ContinuousBuffer.GetSize() == XN_INPUT_ELEMENT_SIZE) { // process it XnUInt32 nActualRead = 0; Unpack11to16(m_ContinuousBuffer.GetData(), XN_INPUT_ELEMENT_SIZE, &nActualRead); m_ContinuousBuffer.Reset(); } } // find out the number of input elements we have XnUInt32 nActualRead = 0; nRetVal = Unpack11to16(pData, nDataSize, &nActualRead); if (nRetVal == XN_STATUS_OK) { pData += nActualRead; nDataSize -= nActualRead; // if we have any bytes left, store them for next packet. if (nDataSize > 0) { // no need to check for overflow. there can not be a case in which more than XN_INPUT_ELEMENT_SIZE // are left. m_ContinuousBuffer.UnsafeWrite(pData, nDataSize); } } XN_PROFILING_END_SECTION }
void XnUncompressedDepthProcessor::ProcessFramePacketChunk(const XnSensorProtocolResponseHeader* /*pHeader*/, const XnUChar* pData, XnUInt32 /*nDataOffset*/, XnUInt32 nDataSize) { XN_PROFILING_START_SECTION("XnUncompressedDepthProcessor::ProcessFramePacketChunk") // when depth is uncompressed, we can just copy it directly to write buffer XnBuffer* pWriteBuffer = GetWriteBuffer(); // Check there is enough room for the depth pixels if (CheckDepthBufferForOverflow(nDataSize)) { // sometimes, when packets are lost, we get uneven number of bytes, so we need to complete // one byte, in order to keep UINT16 alignment if (nDataSize % 2 != 0) { nDataSize--; pData++; } // copy values. Make sure we do not get corrupted shifts XnUInt16* pRaw = (XnUInt16*)(pData); XnUInt16* pRawEnd = (XnUInt16*)(pData + nDataSize); OniDepthPixel* pDepthBuf = GetDepthOutputBuffer(); OniDepthPixel* pShiftBuf = GetShiftsOutputBuffer(); XnUInt16 shift; while (pRaw < pRawEnd) { shift = (((*pRaw) < (XN_DEVICE_SENSOR_MAX_SHIFT_VALUE-1)) ? (*pRaw) : 0); *pShiftBuf = shift; *pDepthBuf = GetOutput(shift); ++pRaw; ++pDepthBuf; ++pShiftBuf; } pWriteBuffer->UnsafeUpdateSize(nDataSize); } XN_PROFILING_END_SECTION }
void XnPSCompressedDepthProcessor::ProcessFramePacketChunk(const XnSensorProtocolResponseHeader* pHeader, const XnUChar* pData, XnUInt32 nDataOffset, XnUInt32 nDataSize) { XN_PROFILING_START_SECTION("XnPSCompressedDepthProcessor::ProcessFramePacketChunk") XnBuffer* pWriteBuffer = GetWriteBuffer(); const XnUChar* pBuf = NULL; XnUInt32 nBufSize = 0; // check if we have bytes stored from previous calls if (m_RawData.GetSize() > 0) { // we have no choice. We need to append current buffer to previous bytes if (m_RawData.GetFreeSpaceInBuffer() < nDataSize) { xnLogWarning(XN_MASK_SENSOR_PROTOCOL_DEPTH, "Bad overflow depth! %d", m_RawData.GetSize()); FrameIsCorrupted(); } else { m_RawData.UnsafeWrite(pData, nDataSize); } pBuf = m_RawData.GetData(); nBufSize = m_RawData.GetSize(); } else { // we can process the data directly pBuf = pData; nBufSize = nDataSize; } XnUInt32 nOutputSize = pWriteBuffer->GetFreeSpaceInBuffer(); XnUInt32 nWrittenOutput = nOutputSize; XnUInt32 nActualRead = 0; XnBool bLastPart = pHeader->nType == XN_SENSOR_PROTOCOL_RESPONSE_DEPTH_END && (nDataOffset + nDataSize) == pHeader->nBufSize; XnStatus nRetVal = UncompressDepthPS(pBuf, nBufSize, (XnUInt16*)pWriteBuffer->GetUnsafeWritePointer(), &nWrittenOutput, &nActualRead, bLastPart); if (nRetVal != XN_STATUS_OK) { FrameIsCorrupted(); static XnUInt64 nLastPrinted = 0; XnUInt64 nCurrTime; xnOSGetTimeStamp(&nCurrTime); if (nOutputSize != 0 || (nCurrTime - nLastPrinted) > 1000) { xnLogWarning(XN_MASK_SENSOR_PROTOCOL_DEPTH, "Uncompress depth failed: %s. Input Size: %u, Output Space: %u, Last Part: %d.", xnGetStatusString(nRetVal), nBufSize, nOutputSize, bLastPart); xnOSGetTimeStamp(&nLastPrinted); } } pWriteBuffer->UnsafeUpdateSize(nWrittenOutput); nBufSize -= nActualRead; m_RawData.Reset(); // if we have any bytes left, keep them for next time if (nBufSize > 0) { pBuf += nActualRead; m_RawData.UnsafeWrite(pBuf, nBufSize); } XN_PROFILING_END_SECTION }
void XnBayerImageProcessor::ProcessFramePacketChunk(const XnSensorProtocolResponseHeader* pHeader, const XnUChar* pData, XnUInt32 nDataOffset, XnUInt32 nDataSize) { XN_PROFILING_START_SECTION("XnBayerImageProcessor::ProcessFramePacketChunk") // if output format is Gray8, we can write directly to output buffer. otherwise, we need // to write to a temp buffer. XnBuffer* pWriteBuffer = (GetStream()->GetOutputFormat() == XN_OUTPUT_FORMAT_GRAYSCALE8) ? GetWriteBuffer() : &m_UncompressedBayerBuffer; const XnUChar* pBuf = NULL; XnUInt32 nBufSize = 0; // check if we have bytes stored from previous calls if (m_ContinuousBuffer.GetSize() > 0) { // we have no choice. We need to append current buffer to previous bytes if (m_ContinuousBuffer.GetFreeSpaceInBuffer() < nDataSize) { xnLogWarning(XN_MASK_SENSOR_PROTOCOL_DEPTH, "Bad overflow image! %d", m_ContinuousBuffer.GetSize()); FrameIsCorrupted(); } else { m_ContinuousBuffer.UnsafeWrite(pData, nDataSize); } pBuf = m_ContinuousBuffer.GetData(); nBufSize = m_ContinuousBuffer.GetSize(); } else { // we can process the data directly pBuf = pData; nBufSize = nDataSize; } XnUInt32 nOutputSize = pWriteBuffer->GetFreeSpaceInBuffer(); XnUInt32 nWrittenOutput = nOutputSize; XnUInt32 nActualRead = 0; XnBool bLastPart = pHeader->nType == XN_SENSOR_PROTOCOL_RESPONSE_IMAGE_END && (nDataOffset + nDataSize) == pHeader->nBufSize; XnStatus nRetVal = XnStreamUncompressImageNew(pBuf, nBufSize, pWriteBuffer->GetUnsafeWritePointer(), &nWrittenOutput, (XnUInt16)GetActualXRes(), &nActualRead, bLastPart); if (nRetVal != XN_STATUS_OK) { xnLogWarning(XN_MASK_SENSOR_PROTOCOL_IMAGE, "Image decompression failed: %s (%d of %d, requested %d, last %d)", xnGetStatusString(nRetVal), nWrittenOutput, nBufSize, nOutputSize, bLastPart); FrameIsCorrupted(); } pWriteBuffer->UnsafeUpdateSize(nWrittenOutput); nBufSize -= nActualRead; m_ContinuousBuffer.Reset(); // if we have any bytes left, keep them for next time if (nBufSize > 0) { pBuf += nActualRead; m_ContinuousBuffer.UnsafeWrite(pBuf, nBufSize); } XN_PROFILING_END_SECTION }