21 #include "visiontransfer/imageprotocol.h" 22 #include "visiontransfer/alignedallocator.h" 23 #include "visiontransfer/datablockprotocol.h" 24 #include "visiontransfer/exceptions.h" 25 #include "visiontransfer/bitconversions.h" 26 #include "visiontransfer/internalinformation.h" 35 #include <arpa/inet.h> 39 #define LOG_WARN(expr) std::cerr << "DataBlockProtocol: " << expr << std::endl 49 class ImageProtocol::Pimpl {
54 Pimpl(
bool server, ProtocolType protType,
int maxUdpPacketSize);
57 void setTransferImageSet(
const ImageSet& imageSet);
58 void setRawTransferData(
const ImageSet& metaData,
const std::vector<unsigned char*>& rawData,
59 int firstTileWidth = 0,
int middleTilesWidth = 0,
int lastTileWidth = 0,
int validBytes = 0x7FFFFFFF);
60 void setRawValidBytes(
const std::vector<int>& validBytesVec);
61 const unsigned char* getTransferMessage(
int& length);
62 bool transferComplete();
64 bool getReceivedImageSet(
ImageSet& imageSet);
65 bool getPartiallyReceivedImageSet(
ImageSet& imageSet,
66 int& validRows,
bool& complete);
67 bool imagesReceived()
const;
69 unsigned char* getNextReceiveBuffer(
int& maxLength);
71 void processReceivedMessage(
int length);
72 int getProspectiveMessageSize();
73 int getNumDroppedFrames()
const;
74 void resetReception();
75 bool isConnected()
const;
76 const unsigned char* getNextControlMessage(
int& length);
77 bool newClientConnected();
79 std::string statusReport();
82 unsigned short MAGIC_SEQUECE = 0x3D15;
86 struct HeaderDataLegacy {
89 unsigned char protocolVersion;
90 unsigned char isRawImagePair_OBSOLETE;
93 unsigned short height;
95 unsigned short firstTileWidth;
96 unsigned short lastTileWidth;
98 unsigned char format0;
99 unsigned char format1;
100 unsigned short minDisparity;
101 unsigned short maxDisparity;
102 unsigned char subpixelFactor;
110 unsigned short middleTilesWidth;
113 struct HeaderDataV2:
public HeaderDataLegacy {
114 unsigned short totalHeaderSize;
115 unsigned short flags;
116 unsigned char numberOfImages;
117 unsigned char format2;
119 NEW_STYLE_TRANSFER = 1,
125 struct HeaderData:
public HeaderDataV2 {
130 unsigned char imageTypes[8];
136 ProtocolType protType;
139 std::vector<unsigned char> headerBuffer;
142 std::vector<unsigned char, AlignedAllocator<unsigned char> >decodeBuffer[ImageSet::MAX_SUPPORTED_IMAGES];
143 bool receiveHeaderParsed;
144 HeaderData receiveHeader;
145 int lastReceivedPayloadBytes[ImageSet::MAX_SUPPORTED_IMAGES];
149 void copyHeaderToBuffer(
const ImageSet& imageSet,
int firstTileWidth,
150 int middleTilesWidth,
int lastTileWidth,
unsigned char* buffer);
153 void tryDecodeHeader(
const unsigned char* receivedData,
int receivedBytes);
156 unsigned char* decodeNoninterleaved(
int imageNumber,
int numImages,
int receivedBytes,
157 unsigned char* data,
int& validRows,
int& rowStride);
160 unsigned char* decodeInterleaved(
int imageNumber,
int numImages,
int receivedBytes,
161 unsigned char* data,
int& validRows,
int& rowStride);
163 int getNumTiles(
int width,
int firstTileWidth,
int middleTilesWidth,
int lastTileWidth);
165 int getFrameSize(
int width,
int height,
int firstTileWidth,
int middleTilesWidth,
166 int lastTileWidth,
int totalBits);
170 void decodeTiledImage(
int imageNumber,
int lastReceivedPayloadBytes,
int receivedPayloadBytes,
171 const unsigned char* data,
int firstTileStride,
int middleTilesStride,
int lastTileStride,
174 void decodeRowsFromTile(
int startRow,
int stopRow,
unsigned const char* src,
175 unsigned char* dst,
int srcStride,
int dstStride,
int tileWidth);
177 void allocateDecodeBuffer(
int imageNumber);
183 ImageProtocol::ImageProtocol(
bool server,
ProtocolType protType,
int maxUdpPacketSize)
184 : pimpl(new Pimpl(server, protType, maxUdpPacketSize)) {
188 ImageProtocol::~ImageProtocol() {
193 pimpl->setTransferImageSet(imageSet);
197 int firstTileWidth,
int middleTilesWidth,
int lastTileWidth,
int validBytes) {
198 pimpl->setRawTransferData(metaData, imageData, firstTileWidth, middleTilesWidth, lastTileWidth, validBytes);
202 pimpl->setRawValidBytes(validBytesVec);
206 return pimpl->getTransferMessage(length);
210 return pimpl->transferComplete();
214 pimpl->resetTransfer();
218 return pimpl->getReceivedImageSet(imageSet);
222 ImageSet& imageSet,
int& validRows,
bool& complete) {
223 return pimpl->getPartiallyReceivedImageSet(imageSet, validRows, complete);
227 return pimpl->imagesReceived();
231 return pimpl->getNextReceiveBuffer(maxLength);
235 pimpl->processReceivedMessage(length);
239 return pimpl->getNumDroppedFrames();
243 pimpl->resetReception();
247 return pimpl->isConnected();
251 return pimpl->getNextControlMessage(length);
255 return pimpl->newClientConnected();
260 ImageProtocol::Pimpl::Pimpl(
bool server,
ProtocolType protType,
int maxUdpPacketSize)
261 :dataProt(server, (DataBlockProtocol::ProtocolType)protType,
262 maxUdpPacketSize), protType(protType),
263 receiveHeaderParsed(
false), lastReceivedPayloadBytes{0},
264 receptionDone(
false) {
265 headerBuffer.resize(
sizeof(HeaderData) + 128);
266 memset(&headerBuffer[0], 0,
sizeof(headerBuffer.size()));
267 memset(&receiveHeader, 0,
sizeof(receiveHeader));
270 void ImageProtocol::Pimpl::setTransferImageSet(
const ImageSet& imageSet) {
278 copyHeaderToBuffer(imageSet, 0, 0, 0, &headerBuffer[IMAGE_HEADER_OFFSET]);
279 dataProt.resetTransfer();
281 dataProt.setTransferHeader(&headerBuffer[IMAGE_HEADER_OFFSET],
sizeof(HeaderData), numTransferBlocks);
284 int rawDataLength = getFrameSize(imageSet.
getWidth(), imageSet.
getHeight(), 0, 0, 0, bits);
285 dataProt.setTransferBytes(i, rawDataLength);
289 int bits[ImageSet::MAX_SUPPORTED_IMAGES] = {0};
290 int rowSize[ImageSet::MAX_SUPPORTED_IMAGES] = {0};
291 const unsigned char* pixelData[ImageSet::MAX_SUPPORTED_IMAGES] = {
nullptr};
292 std::vector<unsigned char> encodingBuffer[ImageSet::MAX_SUPPORTED_IMAGES];
296 rowSize[i] = imageSet.
getWidth()*bits[i]/8;
301 encodingBuffer[i].resize(rowSize[i] * imageSet.
getHeight());
304 pixelData[i] = &encodingBuffer[i][0];
309 dataProt.setTransferData(i, const_cast<unsigned char*>(pixelData[i]));
313 void ImageProtocol::Pimpl::setRawTransferData(
const ImageSet& metaData,
const std::vector<unsigned char*>& rawData,
314 int firstTileWidth,
int middleTilesWidth,
int lastTileWidth,
int validBytes) {
316 throw ProtocolException(
"Mismatch between metadata and number of image buffers!");
320 copyHeaderToBuffer(metaData, firstTileWidth, middleTilesWidth, lastTileWidth, &headerBuffer[IMAGE_HEADER_OFFSET]);
321 dataProt.resetTransfer();
323 dataProt.setTransferHeader(&headerBuffer[IMAGE_HEADER_OFFSET],
sizeof(HeaderData), numTransferBlocks);
327 firstTileWidth, middleTilesWidth, lastTileWidth, metaData.
getBitsPerPixel(i));
328 dataProt.setTransferBytes(i, rawDataLength);
332 dataProt.setTransferData(i, rawData[i]);
336 void ImageProtocol::Pimpl::setRawValidBytes(
const std::vector<int>& validBytesVec) {
337 for (
int i=0; i<static_cast<int>(validBytesVec.size()); ++i) {
338 dataProt.setTransferValidBytes(i, validBytesVec[i]);
342 const unsigned char* ImageProtocol::Pimpl::getTransferMessage(
int& length) {
343 const unsigned char* msg = dataProt.getTransferMessage(length);
346 msg = dataProt.getTransferMessage(length);
352 bool ImageProtocol::Pimpl::transferComplete() {
353 return dataProt.transferComplete();
356 int ImageProtocol::Pimpl::getNumTiles(
int width,
int firstTileWidth,
int middleTilesWidth,
int lastTileWidth) {
357 if(lastTileWidth == 0) {
359 }
else if(middleTilesWidth == 0) {
362 int tileWidth = firstTileWidth + lastTileWidth - middleTilesWidth;
363 return (width - 2*tileWidth + firstTileWidth + lastTileWidth) / (firstTileWidth + lastTileWidth - tileWidth);
367 int ImageProtocol::Pimpl::getFrameSize(
int width,
int height,
int firstTileWidth,
368 int middleTilesWidth,
int lastTileWidth,
int totalBits) {
369 return (width * height * totalBits) /8;
385 void ImageProtocol::Pimpl::copyHeaderToBuffer(
const ImageSet& imageSet,
386 int firstTileWidth,
int middleTilesWidth,
int lastTileWidth,
unsigned char* buffer) {
387 HeaderData* transferHeader =
reinterpret_cast<HeaderData*
>(buffer);
388 memset(transferHeader, 0,
sizeof(*transferHeader));
389 transferHeader->magic = htons(MAGIC_SEQUECE);
390 transferHeader->protocolVersion = InternalInformation::CURRENT_PROTOCOL_VERSION;
391 transferHeader->isRawImagePair_OBSOLETE = 0;
392 transferHeader->width = htons(imageSet.
getWidth());
393 transferHeader->height = htons(imageSet.
getHeight());
394 transferHeader->firstTileWidth = htons(firstTileWidth);
395 transferHeader->lastTileWidth = htons(lastTileWidth);
396 transferHeader->middleTilesWidth = htons(middleTilesWidth);
397 transferHeader->format0 =
static_cast<unsigned char>(imageSet.
getPixelFormat(0));
399 transferHeader->seqNum =
static_cast<unsigned int>(htonl(imageSet.
getSequenceNumber()));
401 transferHeader->numberOfImages =
static_cast<unsigned char>(imageSet.
getNumberOfImages());
402 transferHeader->totalHeaderSize = htons(
sizeof(HeaderData));
403 transferHeader->flags = htons(HeaderData::FlagBits::NEW_STYLE_TRANSFER | HeaderData::FlagBits::HEADER_V3);
405 int minDisp = 0, maxDisp = 0;
407 transferHeader->minDisparity = minDisp;
408 transferHeader->maxDisparity = maxDisp;
412 int timeSec = 0, timeMicrosec = 0;
414 transferHeader->timeSec =
static_cast<int>(htonl(static_cast<unsigned int>(timeSec)));
415 transferHeader->timeMicrosec =
static_cast<int>(htonl(static_cast<unsigned int>(timeMicrosec)));
417 int numImageChannels = 0;
418 for (
int i=0; i<(int)
sizeof(transferHeader->imageTypes); ++i) {
419 transferHeader->imageTypes[i] =
static_cast<unsigned char>(ImageSet::ImageType::IMAGE_UNDEFINED);
421 int idx = imageSet.
getIndexOf(ImageSet::ImageType::IMAGE_LEFT);
423 transferHeader->imageTypes[idx] =
static_cast<unsigned char>(ImageSet::ImageType::IMAGE_LEFT);
426 idx = imageSet.
getIndexOf(ImageSet::ImageType::IMAGE_RIGHT);
428 transferHeader->imageTypes[idx] =
static_cast<unsigned char>(ImageSet::ImageType::IMAGE_RIGHT);
431 idx = imageSet.
getIndexOf(ImageSet::ImageType::IMAGE_DISPARITY);
433 transferHeader->imageTypes[idx] =
static_cast<unsigned char>(ImageSet::ImageType::IMAGE_DISPARITY);
437 throw std::runtime_error(
"Mismatch between reported number of images and enabled channel selection!");
442 memcpy(transferHeader->q, imageSet.
getQMatrix(),
sizeof(float)*16);
446 void ImageProtocol::Pimpl::resetTransfer() {
447 dataProt.resetTransfer();
450 unsigned char* ImageProtocol::Pimpl::getNextReceiveBuffer(
int& maxLength) {
451 maxLength = dataProt.getMaxReceptionSize();
452 return dataProt.getNextReceiveBuffer(maxLength);
455 void ImageProtocol::Pimpl::processReceivedMessage(
int length) {
456 receptionDone =
false;
459 dataProt.processReceivedMessage(length, receptionDone);
461 int receivedBytes = 0;
462 dataProt.getReceivedData(receivedBytes);
465 if(!receiveHeaderParsed) {
467 unsigned char* headerData = dataProt.getReceivedHeader(headerLen);
468 if(headerData !=
nullptr) {
469 tryDecodeHeader(headerData, headerLen);
474 void ImageProtocol::Pimpl::tryDecodeHeader(
const 475 unsigned char* receivedData,
int receivedBytes) {
478 constexpr
int optionalDataSize =
sizeof(receiveHeader.middleTilesWidth);
479 constexpr
int mandatoryDataSize =
static_cast<int>(
sizeof(HeaderDataLegacy)) - optionalDataSize;
480 constexpr
int fullyExtensibleHeaderSize =
static_cast<int>(
sizeof(HeaderDataV2));
481 bool isCompleteHeader =
false;
483 if(receivedBytes >= mandatoryDataSize) {
484 if (receivedBytes < fullyExtensibleHeaderSize) {
485 *(
static_cast<HeaderDataLegacy*
>(&receiveHeader)) = *
reinterpret_cast<const HeaderDataLegacy*
>(receivedData);
487 receiveHeader = *
reinterpret_cast<const HeaderData*
>(receivedData);
488 isCompleteHeader =
true;
490 if(receiveHeader.magic != htons(MAGIC_SEQUECE)) {
496 if(receiveHeader.protocolVersion != InternalInformation::CURRENT_PROTOCOL_VERSION) {
501 receiveHeader.width = ntohs(receiveHeader.width);
502 receiveHeader.height = ntohs(receiveHeader.height);
503 receiveHeader.firstTileWidth = ntohs(receiveHeader.firstTileWidth);
504 receiveHeader.lastTileWidth = ntohs(receiveHeader.lastTileWidth);
506 receiveHeader.timeSec =
static_cast<int>(
507 ntohl(static_cast<unsigned int>(receiveHeader.timeSec)));
508 receiveHeader.timeMicrosec =
static_cast<int>(
509 ntohl(static_cast<unsigned int>(receiveHeader.timeMicrosec)));
510 receiveHeader.seqNum = ntohl(receiveHeader.seqNum);
513 if(receivedBytes >= mandatoryDataSize + optionalDataSize) {
514 receiveHeader.middleTilesWidth = ntohs(receiveHeader.middleTilesWidth);
516 receiveHeader.middleTilesWidth = 0;
518 if (isCompleteHeader) {
520 receiveHeader.totalHeaderSize = ntohs(receiveHeader.totalHeaderSize);
521 receiveHeader.flags = ntohs(receiveHeader.flags);
524 receiveHeader.totalHeaderSize = (receivedBytes <= mandatoryDataSize) ? mandatoryDataSize : static_cast<int>(
sizeof(HeaderDataLegacy));
525 receiveHeader.flags = 0;
526 receiveHeader.numberOfImages = 2;
527 receiveHeader.format2 = 0;
530 receiveHeaderParsed =
true;
534 bool ImageProtocol::Pimpl::imagesReceived()
const {
535 return receptionDone && receiveHeaderParsed;
538 bool ImageProtocol::Pimpl::getReceivedImageSet(
ImageSet& imageSet) {
539 bool complete =
false;
543 return (ok && complete);
546 bool ImageProtocol::Pimpl::getPartiallyReceivedImageSet(
ImageSet& imageSet,
int& validRows,
bool& complete) {
552 if(!receiveHeaderParsed) {
558 bool flaggedDisparityPair = (receiveHeader.isRawImagePair_OBSOLETE == 0);
559 bool isInterleaved = (receiveHeader.flags & HeaderData::FlagBits::NEW_STYLE_TRANSFER) == 0;
560 bool arbitraryChannels = (receiveHeader.flags & HeaderData::FlagBits::HEADER_V3) > 0;
563 unsigned short unaccountedFlags = receiveHeader.flags & ~(HeaderData::FlagBits::NEW_STYLE_TRANSFER | HeaderData::FlagBits::HEADER_V3);
564 if (unaccountedFlags != 0) {
567 static bool warnedOnceForward =
false;
568 if (!warnedOnceForward) {
569 LOG_WARN(
"Warning: forward-compatible mode; will attempt to process image stream with unknown extra flags. Consider upgrading the client software.");
570 warnedOnceForward =
true;
574 imageSet.
setWidth(receiveHeader.width);
575 imageSet.
setHeight(receiveHeader.height);
576 imageSet.
setPixelFormat(0, static_cast<ImageSet::ImageFormat>(receiveHeader.format0));
580 int rowStrideArr[ImageSet::MAX_SUPPORTED_IMAGES] = {0};
581 int validRowsArr[ImageSet::MAX_SUPPORTED_IMAGES] = {0};
582 unsigned char* pixelArr[ImageSet::MAX_SUPPORTED_IMAGES] = {
nullptr};
586 static bool warnedOnceBackward =
false;
587 if (!warnedOnceBackward) {
588 LOG_WARN(
"Info: backward-compatible mode; the device is sending with a legacy protocol. Consider upgrading its firmware.");
589 warnedOnceBackward =
true;
591 unsigned char* data = dataProt.getBlockReceiveBuffer(0);
592 int validBytes = dataProt.getBlockValidSize(0);
593 for (
int i=0; i < 2; ++i) {
594 pixelArr[i] = decodeInterleaved(i, imageSet.
getNumberOfImages(), validBytes, data, validRowsArr[i], rowStrideArr[i]);
597 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_LEFT, 0);
598 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_RIGHT, flaggedDisparityPair ? -1 : 1);
599 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_DISPARITY, flaggedDisparityPair ? 1 : -1);
602 for (
int i=0; i<receiveHeader.numberOfImages; ++i) {
603 unsigned char* data = dataProt.getBlockReceiveBuffer(i);
604 int validBytes = dataProt.getBlockValidSize(i);
605 pixelArr[i] = decodeNoninterleaved(i, imageSet.
getNumberOfImages(), validBytes, data, validRowsArr[i], rowStrideArr[i]);
607 if (arbitraryChannels) {
609 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_LEFT, -1);
610 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_RIGHT, -1);
611 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_DISPARITY, -1);
613 int typ = receiveHeader.imageTypes[i];
618 static bool warnedOnceV2 =
false;
620 LOG_WARN(
"Info: received a transfer with header v2");
625 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_LEFT, 0);
626 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_RIGHT, flaggedDisparityPair ? -1 : 1);
627 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_DISPARITY, flaggedDisparityPair ? 1 : -1);
631 for (
int i=0; i<receiveHeader.numberOfImages; ++i) {
638 imageSet.
setTimestamp(receiveHeader.timeSec, receiveHeader.timeMicrosec);
639 imageSet.
setDisparityRange(receiveHeader.minDisparity, receiveHeader.maxDisparity);
642 validRows = validRowsArr[0];
643 for (
int i=0; i<receiveHeader.numberOfImages; ++i) {
644 if (validRowsArr[i] < validRows) {
645 validRows = validRowsArr[i];
649 if(validRows == receiveHeader.height || receptionDone) {
658 unsigned char* ImageProtocol::Pimpl::decodeNoninterleaved(
int imageNumber,
int numImages,
int receivedBytes,
659 unsigned char* data,
int& validRows,
int& rowStride) {
662 switch (imageNumber) {
676 throw ProtocolException(
"Not implemented: decodeNoninterleaved with image index > 2");
678 bits = getFormatBits(static_cast<ImageSet::ImageFormat>(format),
false);
680 int totalBits = bits;
681 unsigned char* ret =
nullptr;
683 if(receiveHeader.lastTileWidth == 0) {
684 int bufferOffset0 = 0;
685 int bufferRowStride = receiveHeader.width*(totalBits) / 8;
690 ret = &data[bufferOffset0];
691 rowStride = bufferRowStride;
692 validRows = receivedBytes / bufferRowStride;
695 allocateDecodeBuffer(imageNumber);
696 validRows = receivedBytes / bufferRowStride;
697 rowStride = 2*receiveHeader.width;
698 int lastRow = lastReceivedPayloadBytes[imageNumber] / bufferRowStride;
700 BitConversions::decode12BitPacked(lastRow, validRows, &data[bufferOffset0],
701 &decodeBuffer[imageNumber][0], bufferRowStride, rowStride, receiveHeader.width);
703 ret = &decodeBuffer[imageNumber][0];
707 decodeTiledImage(imageNumber,
708 lastReceivedPayloadBytes[imageNumber], receivedBytes, data,
709 receiveHeader.firstTileWidth * (totalBits) / 8,
710 receiveHeader.middleTilesWidth * (totalBits) / 8,
711 receiveHeader.lastTileWidth * (totalBits) / 8,
712 validRows, format,
false);
713 ret = &decodeBuffer[imageNumber][0];
714 rowStride = receiveHeader.width*getFormatBits(
715 static_cast<ImageSet::ImageFormat>(format),
true)/8;
718 lastReceivedPayloadBytes[imageNumber] = receivedBytes;
723 unsigned char* ImageProtocol::Pimpl::decodeInterleaved(
int imageNumber,
int numImages,
int receivedBytes,
724 unsigned char* data,
int& validRows,
int& rowStride) {
726 imageNumber == 0 ? receiveHeader.format0 : receiveHeader.format1);
727 int bits0 = getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format0),
false);
728 int bits1 = getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format1),
false);
729 int bits2 = getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format2),
false);
731 int totalBits = (numImages<3)?(bits0 + bits1):(bits0 + bits1 + bits2);
733 unsigned char* ret =
nullptr;
735 if(receiveHeader.lastTileWidth == 0) {
737 switch (imageNumber) {
738 case 0: { bufferOffset = 0;
break; }
739 case 1: { bufferOffset = receiveHeader.width * bits0/8;
break; }
740 case 2: { bufferOffset = receiveHeader.width * (bits0 + bits1)/8;
break; }
744 int bufferRowStride = receiveHeader.width*(totalBits) / 8;
749 ret = &data[bufferOffset];
750 rowStride = bufferRowStride;
751 validRows = receivedBytes / bufferRowStride;
754 allocateDecodeBuffer(imageNumber);
755 validRows = receivedBytes / bufferRowStride;
756 rowStride = 2*receiveHeader.width;
757 int lastRow = lastReceivedPayloadBytes[imageNumber] / bufferRowStride;
759 BitConversions::decode12BitPacked(lastRow, validRows, &data[bufferOffset],
760 &decodeBuffer[imageNumber][0], bufferRowStride, rowStride, receiveHeader.width);
762 ret = &decodeBuffer[imageNumber][0];
766 decodeTiledImage(imageNumber,
767 lastReceivedPayloadBytes[imageNumber], receivedBytes, data,
768 receiveHeader.firstTileWidth * (totalBits) / 8,
769 receiveHeader.middleTilesWidth * (totalBits) / 8,
770 receiveHeader.lastTileWidth * (totalBits) / 8,
771 validRows, format,
true);
772 ret = &decodeBuffer[imageNumber][0];
773 rowStride = receiveHeader.width*getFormatBits(
774 static_cast<ImageSet::ImageFormat>(format),
true)/8;
777 lastReceivedPayloadBytes[imageNumber] = receivedBytes;
781 void ImageProtocol::Pimpl::allocateDecodeBuffer(
int imageNumber) {
783 switch (imageNumber) {
797 throw ProtocolException(
"Not implemented: allocateDecodeBuffer with image index > 2");
799 int bitsPerPixel = getFormatBits(format,
true);
800 int bufferSize = receiveHeader.width * receiveHeader.height * bitsPerPixel / 8;
802 if(decodeBuffer[imageNumber].size() != static_cast<unsigned int>(bufferSize)) {
803 decodeBuffer[imageNumber].resize(bufferSize);
807 void ImageProtocol::Pimpl::decodeTiledImage(
int imageNumber,
int lastReceivedPayloadBytes,
int receivedPayloadBytes,
808 const unsigned char* data,
int firstTileStride,
int middleTilesStride,
int lastTileStride,
int& validRows,
811 allocateDecodeBuffer(imageNumber);
814 int numTiles = getNumTiles(receiveHeader.width, receiveHeader.firstTileWidth,
815 receiveHeader.middleTilesWidth, receiveHeader.lastTileWidth);
816 int payloadOffset = 0;
817 int decodeXOffset = 0;
818 int prevTileStrides = 0;
819 for(
int i = 0; i < numTiles; i++) {
825 tileStride = firstTileStride;
826 tileWidth = receiveHeader.firstTileWidth;
827 }
else if(i == numTiles-1) {
828 tileStride = lastTileStride;
829 tileWidth = receiveHeader.lastTileWidth;
831 tileStride = middleTilesStride;
832 tileWidth = receiveHeader.middleTilesWidth;
835 int tileStart = std::max(0, (lastReceivedPayloadBytes - payloadOffset) / tileStride);
836 int tileStop = std::min(std::max(0, (receivedPayloadBytes - payloadOffset) / tileStride), (
int)receiveHeader.height);
838 if (dataIsInterleaved) {
839 switch (imageNumber) {
840 case 0: { tileOffset = 0;
break; }
841 case 1: { tileOffset = tileWidth * (
842 getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format0),
false)
844 case 2: { tileOffset = tileWidth * (
845 getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format0),
false)
846 + getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format1),
false)
855 tileOffset += receiveHeader.height * prevTileStrides;
862 BitConversions::decode12BitPacked(tileStart, tileStop, &data[tileOffset],
863 &decodeBuffer[imageNumber][decodeXOffset], tileStride, 2*receiveHeader.width, tileWidth);
866 decodeRowsFromTile(tileStart, tileStop, &data[tileOffset],
867 &decodeBuffer[imageNumber][decodeXOffset], tileStride,
868 receiveHeader.width*bytesPixel, tileWidth*bytesPixel);
871 payloadOffset += receiveHeader.height * tileStride;
872 decodeXOffset += tileWidth * bytesPixel;
873 prevTileStrides += tileStride;
874 if(i == numTiles-1) {
875 validRows = tileStop;
880 void ImageProtocol::Pimpl::decodeRowsFromTile(
int startRow,
int stopRow,
unsigned const char* src,
881 unsigned char* dst,
int srcStride,
int dstStride,
int tileWidth) {
882 for(
int y = startRow; y < stopRow; y++) {
883 memcpy(&dst[y*dstStride], &src[y*srcStride], tileWidth);
887 void ImageProtocol::Pimpl::resetReception() {
888 receiveHeaderParsed =
false;
889 for (
int i=0; i<ImageSet::MAX_SUPPORTED_IMAGES; ++i) {
890 lastReceivedPayloadBytes[i] = 0;
892 dataProt.resetReception(
false);
893 receptionDone =
false;
896 bool ImageProtocol::Pimpl::isConnected()
const {
897 return dataProt.isConnected();
900 const unsigned char* ImageProtocol::Pimpl::getNextControlMessage(
int& length) {
901 return dataProt.getNextControlMessage(length);
904 bool ImageProtocol::Pimpl::newClientConnected() {
905 return dataProt.newClientConnected();
908 int ImageProtocol::Pimpl::getNumDroppedFrames()
const {
909 return dataProt.getDroppedReceptions();
912 std::string ImageProtocol::statusReport() {
913 return pimpl->statusReport();
915 std::string ImageProtocol::Pimpl::statusReport() {
916 return dataProt.statusReport();
void resetTransfer()
Aborts the transmission of the current transfer and performs a reset of the internal state...
void setRawTransferData(const ImageSet &metaData, const std::vector< unsigned char *> &imageData, int firstTileWidth=0, int middleTilesWidth=0, int lastTileWidth=0, int validBytes=0x7FFFFFFF)
Sets the already pre-formatted image data for the next transfer.
unsigned char * getNextReceiveBuffer(int &maxLength)
Returns the buffer for receiving the next network message.
void setNumberOfImages(int number)
Sets the number of valid images in this set.
int getRowStride(int imageNumber) const
Returns the row stride for the pixel data of one image.
void setTimestamp(int seconds, int microsec)
Sets the time at which this image set has been captured.
void setPixelData(int imageNumber, unsigned char *pixelData)
Sets the pixel data for the given image.
void getTimestamp(int &seconds, int µsec) const
Returns the time at which this image set has been captured.
void processReceivedMessage(int length)
Handles a received network message.
const float * getQMatrix() const
Returns a pointer to the disparity-to-depth mapping matrix q.
void setWidth(int w)
Sets a new width for both images.
int getBitsPerPixel(int imageNumber) const
Returns the number of bits that are required to store one image pixel.
int getIndexOf(ImageType what, bool throwIfNotFound=false) const
Returns the index of a specific image type.
int getNumberOfImages() const
Returns the number of images in this set.
unsigned int getSequenceNumber() const
Returns the sequence number for this image set.
void setSequenceNumber(unsigned int num)
Sets the sequence number for this image set.
int getHeight() const
Returns the height of each image.
void setHeight(int h)
Sets a new width for both images.
int getBytesPerPixel(int imageNumber) const
Returns the number of bytes that are required to store one image pixel.
bool isConnected() const
Returns true if a remote connection is established.
int getSubpixelFactor() const
Gets the subpixel factor for this image set.
const unsigned char * getNextControlMessage(int &length)
If a control message is pending to be transmitted then the message data will be returned by this meth...
void setPixelFormat(int imageNumber, ImageFormat format)
Sets the pixel format for the given image.
bool transferComplete()
Returns true if the current transfer has been completed.
bool getPartiallyReceivedImageSet(ImageSet &imageSet, int &validRows, bool &complete)
Returns a partially received image.
ProtocolType
Supported network protocols.
void setRowStride(int imageNumber, int stride)
Sets a new row stride for the pixel data of one image.
void setTransferImageSet(const ImageSet &imageSet)
Sets a new image that will be transfer.
bool imagesReceived() const
Returns true if the images of the current transfer have been received.
void getDisparityRange(int &minimum, int &maximum) const
Gets the value range for the disparity map contained in this image set. If the image set does not con...
ImageType
Supported image types.
ImageFormat
Image formats that can be transferred.
bool getReceivedImageSet(ImageSet &imageSet)
Returns a received image when complete.
unsigned char * getPixelData(int imageNumber) const
Returns the pixel data for the given image.
A set of one to three images, but usually two (the left camera image and the disparity map)...
ImageFormat getPixelFormat(int imageNumber) const
Returns the pixel format for the given image.
const unsigned char * getTransferMessage(int &length)
Gets the next network message for the current transfer.
A protocol for transmitting large blocks of data over a network.
bool newClientConnected()
Returns true if the last message has established a new connection from a client.
int getNumDroppedFrames() const
Returns the number of frames that have been dropped since connecting to the current remote host...
void setIndexOf(ImageType what, int idx)
Assign an image index to a specified ImageType, -1 to disable.
void setRawValidBytes(const std::vector< int > &validBytes)
Updates the number of valid bytes in a partial raw transfer.
void setDisparityRange(int minimum, int maximum)
Sets the value range for the disparity map contained in this image set.
Exception class that is used for all protocol exceptions.
int getWidth() const
Returns the width of each image.
void resetReception()
Aborts the reception of the current image transfer and resets the internal state. ...
void setSubpixelFactor(int subpixFact)
Sets the subpixel factor for this image set.
void setQMatrix(const float *q)
Sets the pointer to the disparity-to-depth mapping matrix q.