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
}
