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> 38 #define LOG_DEBUG_IMPROTO(expr) 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);
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,
127 struct HeaderDataV3:
public HeaderDataV2 {
132 unsigned char imageTypes[8];
135 struct HeaderDataV4:
public HeaderDataV3 {
137 int lastSyncPulseSec;
138 int lastSyncPulseMicrosec;
141 struct HeaderData:
public HeaderDataV4{
142 unsigned char format3;
148 ProtocolType protType;
151 std::vector<unsigned char> headerBuffer;
154 std::vector<unsigned char, AlignedAllocator<unsigned char> >decodeBuffer[ImageSet::MAX_SUPPORTED_IMAGES];
155 bool receiveHeaderParsed;
156 HeaderData receiveHeader;
157 int lastReceivedPayloadBytes[ImageSet::MAX_SUPPORTED_IMAGES];
161 void copyHeaderToBuffer(
const ImageSet& imageSet,
int firstTileWidth,
162 int middleTilesWidth,
int lastTileWidth,
unsigned char* buffer);
165 void tryDecodeHeader(
const unsigned char* receivedData,
int receivedBytes);
168 unsigned char* decodeNoninterleaved(
int imageNumber,
int numImages,
int receivedBytes,
169 unsigned char* data,
int& validRows,
int& rowStride);
172 unsigned char* decodeInterleaved(
int imageNumber,
int numImages,
int receivedBytes,
173 unsigned char* data,
int& validRows,
int& rowStride);
175 int getNumTiles(
int width,
int firstTileWidth,
int middleTilesWidth,
int lastTileWidth);
177 int getFrameSize(
int width,
int height,
int firstTileWidth,
int middleTilesWidth,
178 int lastTileWidth,
int totalBits);
182 void decodeTiledImage(
int imageNumber,
int lastReceivedPayloadBytes,
int receivedPayloadBytes,
183 const unsigned char* data,
int firstTileStride,
int middleTilesStride,
int lastTileStride,
186 void decodeRowsFromTile(
int startRow,
int stopRow,
unsigned const char* src,
187 unsigned char* dst,
int srcStride,
int dstStride,
int tileWidth);
189 void allocateDecodeBuffer(
int imageNumber);
195 ImageProtocol::ImageProtocol(
bool server,
ProtocolType protType,
int maxUdpPacketSize)
196 : pimpl(new Pimpl(server, protType, maxUdpPacketSize)) {
200 ImageProtocol::~ImageProtocol() {
205 pimpl->setTransferImageSet(imageSet);
209 int firstTileWidth,
int middleTilesWidth,
int lastTileWidth) {
210 pimpl->setRawTransferData(metaData, imageData, firstTileWidth, middleTilesWidth, lastTileWidth);
214 pimpl->setRawValidBytes(validBytesVec);
218 return pimpl->getTransferMessage(length);
222 return pimpl->transferComplete();
226 pimpl->resetTransfer();
230 return pimpl->getReceivedImageSet(imageSet);
234 ImageSet& imageSet,
int& validRows,
bool& complete) {
235 return pimpl->getPartiallyReceivedImageSet(imageSet, validRows, complete);
239 return pimpl->imagesReceived();
243 return pimpl->getNextReceiveBuffer(maxLength);
247 pimpl->processReceivedMessage(length);
251 return pimpl->getNumDroppedFrames();
255 pimpl->resetReception();
259 return pimpl->isConnected();
263 return pimpl->getNextControlMessage(length);
267 return pimpl->newClientConnected();
272 ImageProtocol::Pimpl::Pimpl(
bool server,
ProtocolType protType,
int maxUdpPacketSize)
273 :dataProt(server, (DataBlockProtocol::ProtocolType)protType,
274 maxUdpPacketSize), protType(protType),
275 receiveHeaderParsed(
false), lastReceivedPayloadBytes{0},
276 receptionDone(
false) {
277 headerBuffer.resize(
sizeof(HeaderData) + 128);
278 memset(&headerBuffer[0], 0,
sizeof(headerBuffer.size()));
279 memset(&receiveHeader, 0,
sizeof(receiveHeader));
282 void ImageProtocol::Pimpl::setTransferImageSet(
const ImageSet& imageSet) {
290 copyHeaderToBuffer(imageSet, 0, 0, 0, &headerBuffer[IMAGE_HEADER_OFFSET]);
291 dataProt.resetTransfer();
293 dataProt.setTransferHeader(&headerBuffer[IMAGE_HEADER_OFFSET],
sizeof(HeaderData), numTransferBlocks);
296 int rawDataLength = getFrameSize(imageSet.
getWidth(), imageSet.
getHeight(), 0, 0, 0, bits);
297 dataProt.setTransferBytes(i, rawDataLength);
301 int bits[ImageSet::MAX_SUPPORTED_IMAGES] = {0};
302 int rowSize[ImageSet::MAX_SUPPORTED_IMAGES] = {0};
303 const unsigned char* pixelData[ImageSet::MAX_SUPPORTED_IMAGES] = {
nullptr};
307 rowSize[i] = imageSet.
getWidth()*bits[i]/8;
312 static std::vector<unsigned char> encodingBuffer[ImageSet::MAX_SUPPORTED_IMAGES];
313 encodingBuffer[i].resize(rowSize[i] * imageSet.
getHeight());
316 pixelData[i] = &encodingBuffer[i][0];
321 dataProt.setTransferData(i, const_cast<unsigned char*>(pixelData[i]));
325 void ImageProtocol::Pimpl::setRawTransferData(
const ImageSet& metaData,
const std::vector<unsigned char*>& rawData,
326 int firstTileWidth,
int middleTilesWidth,
int lastTileWidth) {
328 throw ProtocolException(
"Mismatch between metadata and number of image buffers!");
332 copyHeaderToBuffer(metaData, firstTileWidth, middleTilesWidth, lastTileWidth, &headerBuffer[IMAGE_HEADER_OFFSET]);
333 dataProt.resetTransfer();
335 dataProt.setTransferHeader(&headerBuffer[IMAGE_HEADER_OFFSET],
sizeof(HeaderData), numTransferBlocks);
339 firstTileWidth, middleTilesWidth, lastTileWidth, metaData.
getBitsPerPixel(i));
340 dataProt.setTransferBytes(i, rawDataLength);
344 dataProt.setTransferData(i, rawData[i]);
348 void ImageProtocol::Pimpl::setRawValidBytes(
const std::vector<int>& validBytesVec) {
349 for (
int i=0; i<static_cast<int>(validBytesVec.size()); ++i) {
350 dataProt.setTransferValidBytes(i, validBytesVec[i]);
354 const unsigned char* ImageProtocol::Pimpl::getTransferMessage(
int& length) {
355 const unsigned char* msg = dataProt.getTransferMessage(length);
358 msg = dataProt.getTransferMessage(length);
364 bool ImageProtocol::Pimpl::transferComplete() {
365 return dataProt.transferComplete();
368 int ImageProtocol::Pimpl::getNumTiles(
int width,
int firstTileWidth,
int middleTilesWidth,
int lastTileWidth) {
369 if(lastTileWidth == 0) {
371 }
else if(middleTilesWidth == 0) {
374 int tileWidth = firstTileWidth + lastTileWidth - middleTilesWidth;
375 return (width - 2*tileWidth + firstTileWidth + lastTileWidth) / (firstTileWidth + lastTileWidth - tileWidth);
379 int ImageProtocol::Pimpl::getFrameSize(
int width,
int height,
int firstTileWidth,
380 int middleTilesWidth,
int lastTileWidth,
int totalBits) {
381 return (width * height * totalBits) /8;
397 void ImageProtocol::Pimpl::copyHeaderToBuffer(
const ImageSet& imageSet,
398 int firstTileWidth,
int middleTilesWidth,
int lastTileWidth,
unsigned char* buffer) {
399 int timeSec = 0, timeMicrosec = 0;
400 HeaderData* transferHeader =
reinterpret_cast<HeaderData*
>(buffer);
402 memset(transferHeader, 0,
sizeof(*transferHeader));
403 transferHeader->magic = htons(MAGIC_SEQUECE);
404 transferHeader->protocolVersion = InternalInformation::CURRENT_PROTOCOL_VERSION;
405 transferHeader->isRawImagePair_OBSOLETE = 0;
406 transferHeader->width = htons(imageSet.
getWidth());
407 transferHeader->height = htons(imageSet.
getHeight());
408 transferHeader->firstTileWidth = htons(firstTileWidth);
409 transferHeader->lastTileWidth = htons(lastTileWidth);
410 transferHeader->middleTilesWidth = htons(middleTilesWidth);
411 transferHeader->format0 =
static_cast<unsigned char>(imageSet.
getPixelFormat(0));
413 transferHeader->seqNum =
static_cast<unsigned int>(htonl(imageSet.
getSequenceNumber()));
416 transferHeader->numberOfImages =
static_cast<unsigned char>(imageSet.
getNumberOfImages());
420 transferHeader->lastSyncPulseSec = htonl(timeSec);
421 transferHeader->lastSyncPulseMicrosec = htonl(timeMicrosec);
423 transferHeader->totalHeaderSize = htons(
sizeof(HeaderData));
424 transferHeader->flags = htons(HeaderData::FlagBits::NEW_STYLE_TRANSFER | HeaderData::FlagBits::HEADER_V3
425 | HeaderData::FlagBits::HEADER_V4 | HeaderData::FlagBits::HEADER_V5);
427 int minDisp = 0, maxDisp = 0;
429 transferHeader->minDisparity = minDisp;
430 transferHeader->maxDisparity = maxDisp;
435 transferHeader->timeSec =
static_cast<int>(htonl(static_cast<unsigned int>(timeSec)));
436 transferHeader->timeMicrosec =
static_cast<int>(htonl(static_cast<unsigned int>(timeMicrosec)));
438 int numImageChannels = 0;
439 for (
int i=0; i<(int)
sizeof(transferHeader->imageTypes); ++i) {
440 transferHeader->imageTypes[i] =
static_cast<unsigned char>(ImageSet::ImageType::IMAGE_UNDEFINED);
442 int idx = imageSet.
getIndexOf(ImageSet::ImageType::IMAGE_LEFT);
444 transferHeader->imageTypes[idx] =
static_cast<unsigned char>(ImageSet::ImageType::IMAGE_LEFT);
447 idx = imageSet.
getIndexOf(ImageSet::ImageType::IMAGE_RIGHT);
449 transferHeader->imageTypes[idx] =
static_cast<unsigned char>(ImageSet::ImageType::IMAGE_RIGHT);
452 idx = imageSet.
getIndexOf(ImageSet::ImageType::IMAGE_DISPARITY);
454 transferHeader->imageTypes[idx] =
static_cast<unsigned char>(ImageSet::ImageType::IMAGE_DISPARITY);
457 idx = imageSet.
getIndexOf(ImageSet::ImageType::IMAGE_COLOR);
459 transferHeader->imageTypes[idx] =
static_cast<unsigned char>(ImageSet::ImageType::IMAGE_COLOR);
463 throw std::runtime_error(
"Mismatch between reported number of images and enabled channel selection!");
468 memcpy(transferHeader->q, imageSet.
getQMatrix(),
sizeof(float)*16);
472 void ImageProtocol::Pimpl::resetTransfer() {
473 dataProt.resetTransfer();
476 unsigned char* ImageProtocol::Pimpl::getNextReceiveBuffer(
int& maxLength) {
477 maxLength = dataProt.getMaxReceptionSize();
478 return dataProt.getNextReceiveBuffer(maxLength);
481 void ImageProtocol::Pimpl::processReceivedMessage(
int length) {
482 receptionDone =
false;
485 dataProt.processReceivedMessage(length, receptionDone);
486 if(!dataProt.wasHeaderReceived() && receiveHeaderParsed) {
488 LOG_DEBUG_IMPROTO(
"Resetting image protocol!");
493 int receivedBytes = 0;
494 dataProt.getReceivedData(receivedBytes);
497 if(!receiveHeaderParsed) {
499 unsigned char* headerData = dataProt.getReceivedHeader(headerLen);
500 if(headerData !=
nullptr) {
501 tryDecodeHeader(headerData, headerLen);
506 void ImageProtocol::Pimpl::tryDecodeHeader(
const 507 unsigned char* receivedData,
int receivedBytes) {
510 constexpr
int optionalDataSize =
sizeof(receiveHeader.middleTilesWidth);
511 constexpr
int mandatoryDataSize =
static_cast<int>(
sizeof(HeaderDataLegacy)) - optionalDataSize;
512 constexpr
int fullyExtensibleHeaderSize =
static_cast<int>(
sizeof(HeaderDataV2));
513 bool isCompleteHeader =
false;
515 if(receivedBytes >= mandatoryDataSize) {
516 if (receivedBytes < fullyExtensibleHeaderSize) {
517 *(
static_cast<HeaderDataLegacy*
>(&receiveHeader)) = *
reinterpret_cast<const HeaderDataLegacy*
>(receivedData);
519 memcpy(&receiveHeader, receivedData, std::min((
size_t)receivedBytes,
sizeof(HeaderData)));
520 receiveHeader = *
reinterpret_cast<const HeaderData*
>(receivedData);
521 isCompleteHeader =
true;
523 if(receiveHeader.magic != htons(MAGIC_SEQUECE)) {
529 if(receiveHeader.protocolVersion != InternalInformation::CURRENT_PROTOCOL_VERSION) {
534 receiveHeader.width = ntohs(receiveHeader.width);
535 receiveHeader.height = ntohs(receiveHeader.height);
536 receiveHeader.firstTileWidth = ntohs(receiveHeader.firstTileWidth);
537 receiveHeader.lastTileWidth = ntohs(receiveHeader.lastTileWidth);
539 receiveHeader.timeSec =
static_cast<int>(
540 ntohl(static_cast<unsigned int>(receiveHeader.timeSec)));
541 receiveHeader.timeMicrosec =
static_cast<int>(
542 ntohl(static_cast<unsigned int>(receiveHeader.timeMicrosec)));
543 receiveHeader.seqNum = ntohl(receiveHeader.seqNum);
546 if(receivedBytes >= mandatoryDataSize + optionalDataSize) {
547 receiveHeader.middleTilesWidth = ntohs(receiveHeader.middleTilesWidth);
549 receiveHeader.middleTilesWidth = 0;
551 if (isCompleteHeader) {
553 receiveHeader.totalHeaderSize = ntohs(receiveHeader.totalHeaderSize);
554 receiveHeader.flags = ntohs(receiveHeader.flags);
555 receiveHeader.exposureTime = ntohl(receiveHeader.exposureTime);
556 receiveHeader.lastSyncPulseSec = htonl(receiveHeader.lastSyncPulseSec);
557 receiveHeader.lastSyncPulseMicrosec = htonl(receiveHeader.lastSyncPulseMicrosec);
560 receiveHeader.totalHeaderSize = (receivedBytes <= mandatoryDataSize) ? mandatoryDataSize : static_cast<int>(
sizeof(HeaderDataLegacy));
561 receiveHeader.flags = 0;
562 receiveHeader.numberOfImages = 2;
563 receiveHeader.format2 = 0;
564 receiveHeader.format3 = 0;
565 receiveHeader.exposureTime = 0;
566 receiveHeader.lastSyncPulseSec = 0;
567 receiveHeader.lastSyncPulseMicrosec = 0;
570 receiveHeaderParsed =
true;
574 bool ImageProtocol::Pimpl::imagesReceived()
const {
575 return receptionDone && receiveHeaderParsed;
578 bool ImageProtocol::Pimpl::getReceivedImageSet(
ImageSet& imageSet) {
579 bool complete =
false;
583 return (ok && complete);
586 bool ImageProtocol::Pimpl::getPartiallyReceivedImageSet(
ImageSet& imageSet,
int& validRows,
bool& complete) {
592 if(!receiveHeaderParsed) {
598 bool flaggedDisparityPair = (receiveHeader.isRawImagePair_OBSOLETE == 0);
599 bool isInterleaved = (receiveHeader.flags & HeaderData::FlagBits::NEW_STYLE_TRANSFER) == 0;
600 bool arbitraryChannels = (receiveHeader.flags & HeaderData::FlagBits::HEADER_V3) > 0;
601 bool hasExposureTime = (receiveHeader.flags & HeaderData::FlagBits::HEADER_V4) > 0;
604 unsigned short unaccountedFlags = receiveHeader.flags & ~(HeaderData::FlagBits::NEW_STYLE_TRANSFER
605 | HeaderData::FlagBits::HEADER_V3 | HeaderData::FlagBits::HEADER_V4 | HeaderData::FlagBits::HEADER_V5);
606 if (unaccountedFlags != 0) {
609 static bool warnedOnceForward =
false;
610 if (!warnedOnceForward) {
611 LOG_DEBUG_IMPROTO(
"Warning: forward-compatible mode; will attempt to process image stream with unknown extra flags. Consider upgrading the client software.");
612 warnedOnceForward =
true;
616 imageSet.
setWidth(receiveHeader.width);
617 imageSet.
setHeight(receiveHeader.height);
619 imageSet.
setPixelFormat(0, static_cast<ImageSet::ImageFormat>(receiveHeader.format0));
624 int rowStrideArr[ImageSet::MAX_SUPPORTED_IMAGES] = {0};
625 int validRowsArr[ImageSet::MAX_SUPPORTED_IMAGES] = {0};
626 unsigned char* pixelArr[ImageSet::MAX_SUPPORTED_IMAGES] = {
nullptr};
630 static bool warnedOnceBackward =
false;
631 if (!warnedOnceBackward) {
632 LOG_DEBUG_IMPROTO(
"Info: backward-compatible mode; the device is sending with a legacy protocol. Consider upgrading its firmware.");
633 warnedOnceBackward =
true;
635 unsigned char* data = dataProt.getBlockReceiveBuffer(0);
636 int validBytes = dataProt.getBlockValidSize(0);
637 for (
int i=0; i < 2; ++i) {
638 pixelArr[i] = decodeInterleaved(i, imageSet.
getNumberOfImages(), validBytes, data, validRowsArr[i], rowStrideArr[i]);
641 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_LEFT, 0);
642 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_RIGHT, flaggedDisparityPair ? -1 : 1);
643 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_DISPARITY, flaggedDisparityPair ? 1 : -1);
644 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_COLOR, -1);
648 for (
int i=0; i<receiveHeader.numberOfImages; ++i) {
649 unsigned char* data = dataProt.getBlockReceiveBuffer(i);
650 int validBytes = dataProt.getBlockValidSize(i);
651 pixelArr[i] = decodeNoninterleaved(i, imageSet.
getNumberOfImages(), validBytes, data, validRowsArr[i], rowStrideArr[i]);
654 LOG_DEBUG_IMPROTO(
"Protocol exception: " << ex.what());
658 if (arbitraryChannels) {
660 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_LEFT, -1);
661 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_RIGHT, -1);
662 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_DISPARITY, -1);
663 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_COLOR, -1);
665 int typ = receiveHeader.imageTypes[i];
670 static bool warnedOnceV2 =
false;
672 LOG_DEBUG_IMPROTO(
"Info: received a transfer with header v2");
677 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_LEFT, 0);
678 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_RIGHT, flaggedDisparityPair ? -1 : 1);
679 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_DISPARITY, flaggedDisparityPair ? 1 : -1);
680 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_COLOR, -1);
682 if(hasExposureTime) {
684 imageSet.
setLastSyncPulse(receiveHeader.lastSyncPulseSec, receiveHeader.lastSyncPulseMicrosec);
688 for (
int i=0; i<receiveHeader.numberOfImages; ++i) {
695 imageSet.
setTimestamp(receiveHeader.timeSec, receiveHeader.timeMicrosec);
696 imageSet.
setDisparityRange(receiveHeader.minDisparity, receiveHeader.maxDisparity);
699 validRows = validRowsArr[0];
700 for (
int i=0; i<receiveHeader.numberOfImages; ++i) {
701 if (validRowsArr[i] < validRows) {
702 validRows = validRowsArr[i];
706 if(validRows == receiveHeader.height || receptionDone) {
715 unsigned char* ImageProtocol::Pimpl::decodeNoninterleaved(
int imageNumber,
int numImages,
int receivedBytes,
716 unsigned char* data,
int& validRows,
int& rowStride) {
719 switch (imageNumber) {
733 throw ProtocolException(
"Not implemented: decodeNoninterleaved with image index > 2");
735 bits = getFormatBits(static_cast<ImageSet::ImageFormat>(format),
false);
737 int totalBits = bits;
738 unsigned char* ret =
nullptr;
740 if(receiveHeader.lastTileWidth == 0) {
741 int bufferOffset0 = 0;
742 int bufferRowStride = receiveHeader.width*(totalBits) / 8;
747 ret = &data[bufferOffset0];
748 rowStride = bufferRowStride;
749 validRows = std::min(receivedBytes / bufferRowStride, (
int)receiveHeader.height);
752 allocateDecodeBuffer(imageNumber);
753 validRows = std::min(receivedBytes / bufferRowStride, (
int)receiveHeader.height);
754 rowStride = 2*receiveHeader.width;
755 int lastRow = std::min(lastReceivedPayloadBytes[imageNumber] / bufferRowStride, validRows);
757 BitConversions::decode12BitPacked(lastRow, validRows, &data[bufferOffset0],
758 &decodeBuffer[imageNumber][0], bufferRowStride, rowStride, receiveHeader.width);
760 ret = &decodeBuffer[imageNumber][0];
764 decodeTiledImage(imageNumber,
765 lastReceivedPayloadBytes[imageNumber], receivedBytes, data,
766 receiveHeader.firstTileWidth * (totalBits) / 8,
767 receiveHeader.middleTilesWidth * (totalBits) / 8,
768 receiveHeader.lastTileWidth * (totalBits) / 8,
769 validRows, format,
false);
770 ret = &decodeBuffer[imageNumber][0];
771 rowStride = receiveHeader.width*getFormatBits(
772 static_cast<ImageSet::ImageFormat>(format),
true)/8;
775 lastReceivedPayloadBytes[imageNumber] = receivedBytes;
780 unsigned char* ImageProtocol::Pimpl::decodeInterleaved(
int imageNumber,
int numImages,
int receivedBytes,
781 unsigned char* data,
int& validRows,
int& rowStride) {
783 imageNumber == 0 ? receiveHeader.format0 : receiveHeader.format1);
784 int bits0 = getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format0),
false);
785 int bits1 = getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format1),
false);
786 int bits2 = getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format2),
false);
787 int bits3 = getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format3),
false);
789 int totalBits = (numImages<3)?(bits0 + bits1):(bits0 + bits1 + bits2 + bits3);
791 unsigned char* ret =
nullptr;
793 if(receiveHeader.lastTileWidth == 0) {
795 switch (imageNumber) {
796 case 0: { bufferOffset = 0;
break; }
797 case 1: { bufferOffset = receiveHeader.width * bits0/8;
break; }
798 case 2: { bufferOffset = receiveHeader.width * (bits0 + bits1)/8;
break; }
802 int bufferRowStride = receiveHeader.width*(totalBits) / 8;
807 ret = &data[bufferOffset];
808 rowStride = bufferRowStride;
809 validRows = receivedBytes / bufferRowStride;
812 allocateDecodeBuffer(imageNumber);
813 validRows = std::min(receivedBytes / bufferRowStride, (
int)receiveHeader.height);
814 rowStride = 2*receiveHeader.width;
815 int lastRow = lastReceivedPayloadBytes[imageNumber] / bufferRowStride;
817 BitConversions::decode12BitPacked(lastRow, validRows, &data[bufferOffset],
818 &decodeBuffer[imageNumber][0], bufferRowStride, rowStride, receiveHeader.width);
820 ret = &decodeBuffer[imageNumber][0];
824 decodeTiledImage(imageNumber,
825 lastReceivedPayloadBytes[imageNumber], receivedBytes, data,
826 receiveHeader.firstTileWidth * (totalBits) / 8,
827 receiveHeader.middleTilesWidth * (totalBits) / 8,
828 receiveHeader.lastTileWidth * (totalBits) / 8,
829 validRows, format,
true);
830 ret = &decodeBuffer[imageNumber][0];
831 rowStride = receiveHeader.width*getFormatBits(
832 static_cast<ImageSet::ImageFormat>(format),
true)/8;
835 lastReceivedPayloadBytes[imageNumber] = receivedBytes;
839 void ImageProtocol::Pimpl::allocateDecodeBuffer(
int imageNumber) {
841 switch (imageNumber) {
855 throw ProtocolException(
"Not implemented: allocateDecodeBuffer with image index > 2");
857 int bitsPerPixel = getFormatBits(format,
true);
858 int bufferSize = receiveHeader.width * receiveHeader.height * bitsPerPixel / 8;
860 if(decodeBuffer[imageNumber].size() != static_cast<unsigned int>(bufferSize)) {
861 decodeBuffer[imageNumber].resize(bufferSize);
865 void ImageProtocol::Pimpl::decodeTiledImage(
int imageNumber,
int lastReceivedPayloadBytes,
int receivedPayloadBytes,
866 const unsigned char* data,
int firstTileStride,
int middleTilesStride,
int lastTileStride,
int& validRows,
869 allocateDecodeBuffer(imageNumber);
872 int numTiles = getNumTiles(receiveHeader.width, receiveHeader.firstTileWidth,
873 receiveHeader.middleTilesWidth, receiveHeader.lastTileWidth);
874 int payloadOffset = 0;
875 int decodeXOffset = 0;
876 int prevTileStrides = 0;
877 for(
int i = 0; i < numTiles; i++) {
883 tileStride = firstTileStride;
884 tileWidth = receiveHeader.firstTileWidth;
885 }
else if(i == numTiles-1) {
886 tileStride = lastTileStride;
887 tileWidth = receiveHeader.lastTileWidth;
889 tileStride = middleTilesStride;
890 tileWidth = receiveHeader.middleTilesWidth;
893 int tileStart = std::max(0, (lastReceivedPayloadBytes - payloadOffset) / tileStride);
894 int tileStop = std::min(std::max(0, (receivedPayloadBytes - payloadOffset) / tileStride), (
int)receiveHeader.height);
896 if (dataIsInterleaved) {
897 switch (imageNumber) {
898 case 0: { tileOffset = 0;
break; }
899 case 1: { tileOffset = tileWidth * (
900 getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format0),
false)
902 case 2: { tileOffset = tileWidth * (
903 getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format0),
false)
904 + getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format1),
false)
913 tileOffset += receiveHeader.height * prevTileStrides;
920 BitConversions::decode12BitPacked(tileStart, tileStop, &data[tileOffset],
921 &decodeBuffer[imageNumber][decodeXOffset], tileStride, 2*receiveHeader.width, tileWidth);
924 decodeRowsFromTile(tileStart, tileStop, &data[tileOffset],
925 &decodeBuffer[imageNumber][decodeXOffset], tileStride,
926 receiveHeader.width*bytesPixel, tileWidth*bytesPixel);
929 payloadOffset += receiveHeader.height * tileStride;
930 decodeXOffset += tileWidth * bytesPixel;
931 prevTileStrides += tileStride;
932 if(i == numTiles-1) {
933 validRows = tileStop;
938 void ImageProtocol::Pimpl::decodeRowsFromTile(
int startRow,
int stopRow,
unsigned const char* src,
939 unsigned char* dst,
int srcStride,
int dstStride,
int tileWidth) {
940 for(
int y = startRow; y < stopRow; y++) {
941 memcpy(&dst[y*dstStride], &src[y*srcStride], tileWidth);
945 void ImageProtocol::Pimpl::resetReception() {
946 receiveHeaderParsed =
false;
947 for (
int i=0; i<ImageSet::MAX_SUPPORTED_IMAGES; ++i) {
948 lastReceivedPayloadBytes[i] = 0;
950 dataProt.resetReception(
false);
951 receptionDone =
false;
954 bool ImageProtocol::Pimpl::isConnected()
const {
955 return dataProt.isConnected();
958 const unsigned char* ImageProtocol::Pimpl::getNextControlMessage(
int& length) {
959 return dataProt.getNextControlMessage(length);
962 bool ImageProtocol::Pimpl::newClientConnected() {
963 return dataProt.newClientConnected();
966 int ImageProtocol::Pimpl::getNumDroppedFrames()
const {
967 return dataProt.getDroppedReceptions();
970 std::string ImageProtocol::statusReport() {
971 return pimpl->statusReport();
973 std::string ImageProtocol::Pimpl::statusReport() {
974 return dataProt.statusReport();
void resetTransfer()
Aborts the transmission of the current transfer and performs a reset of the internal state...
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.
void setExposureTime(int timeMicrosec)
Sets the exposure time that was used for capturing the image set.
int getExposureTime() const
Gets the exposure time in microseconds that was used for capturing the image 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 setRawTransferData(const ImageSet &metaData, const std::vector< unsigned char *> &imageData, int firstTileWidth=0, int middleTilesWidth=0, int lastTileWidth=0)
Sets the already pre-formatted image data for the next transfer.
void setRowStride(int imageNumber, int stride)
Sets a new row stride for the pixel data of one image.
void setLastSyncPulse(int seconds, int microsec)
Sets the timestamp of the last received sync pulse.
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.
void getLastSyncPulse(int &seconds, int µsec) const
Gets the timestamp of the last received sync pulse.
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.