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_WARN(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,
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,
126 struct HeaderDataV3:
public HeaderDataV2 {
131 unsigned char imageTypes[8];
134 struct HeaderData:
public HeaderDataV3 {
136 int lastSyncPulseSec;
137 int lastSyncPulseMicrosec;
143 ProtocolType protType;
146 std::vector<unsigned char> headerBuffer;
149 std::vector<unsigned char, AlignedAllocator<unsigned char> >decodeBuffer[ImageSet::MAX_SUPPORTED_IMAGES];
150 bool receiveHeaderParsed;
151 HeaderData receiveHeader;
152 int lastReceivedPayloadBytes[ImageSet::MAX_SUPPORTED_IMAGES];
156 void copyHeaderToBuffer(
const ImageSet& imageSet,
int firstTileWidth,
157 int middleTilesWidth,
int lastTileWidth,
unsigned char* buffer);
160 void tryDecodeHeader(
const unsigned char* receivedData,
int receivedBytes);
163 unsigned char* decodeNoninterleaved(
int imageNumber,
int numImages,
int receivedBytes,
164 unsigned char* data,
int& validRows,
int& rowStride);
167 unsigned char* decodeInterleaved(
int imageNumber,
int numImages,
int receivedBytes,
168 unsigned char* data,
int& validRows,
int& rowStride);
170 int getNumTiles(
int width,
int firstTileWidth,
int middleTilesWidth,
int lastTileWidth);
172 int getFrameSize(
int width,
int height,
int firstTileWidth,
int middleTilesWidth,
173 int lastTileWidth,
int totalBits);
177 void decodeTiledImage(
int imageNumber,
int lastReceivedPayloadBytes,
int receivedPayloadBytes,
178 const unsigned char* data,
int firstTileStride,
int middleTilesStride,
int lastTileStride,
181 void decodeRowsFromTile(
int startRow,
int stopRow,
unsigned const char* src,
182 unsigned char* dst,
int srcStride,
int dstStride,
int tileWidth);
184 void allocateDecodeBuffer(
int imageNumber);
190 ImageProtocol::ImageProtocol(
bool server,
ProtocolType protType,
int maxUdpPacketSize)
191 : pimpl(new Pimpl(server, protType, maxUdpPacketSize)) {
195 ImageProtocol::~ImageProtocol() {
200 pimpl->setTransferImageSet(imageSet);
204 int firstTileWidth,
int middleTilesWidth,
int lastTileWidth,
int validBytes) {
205 pimpl->setRawTransferData(metaData, imageData, firstTileWidth, middleTilesWidth, lastTileWidth, validBytes);
209 pimpl->setRawValidBytes(validBytesVec);
213 return pimpl->getTransferMessage(length);
217 return pimpl->transferComplete();
221 pimpl->resetTransfer();
225 return pimpl->getReceivedImageSet(imageSet);
229 ImageSet& imageSet,
int& validRows,
bool& complete) {
230 return pimpl->getPartiallyReceivedImageSet(imageSet, validRows, complete);
234 return pimpl->imagesReceived();
238 return pimpl->getNextReceiveBuffer(maxLength);
242 pimpl->processReceivedMessage(length);
246 return pimpl->getNumDroppedFrames();
250 pimpl->resetReception();
254 return pimpl->isConnected();
258 return pimpl->getNextControlMessage(length);
262 return pimpl->newClientConnected();
267 ImageProtocol::Pimpl::Pimpl(
bool server,
ProtocolType protType,
int maxUdpPacketSize)
268 :dataProt(server, (DataBlockProtocol::ProtocolType)protType,
269 maxUdpPacketSize), protType(protType),
270 receiveHeaderParsed(
false), lastReceivedPayloadBytes{0},
271 receptionDone(
false) {
272 headerBuffer.resize(
sizeof(HeaderData) + 128);
273 memset(&headerBuffer[0], 0,
sizeof(headerBuffer.size()));
274 memset(&receiveHeader, 0,
sizeof(receiveHeader));
277 void ImageProtocol::Pimpl::setTransferImageSet(
const ImageSet& imageSet) {
285 copyHeaderToBuffer(imageSet, 0, 0, 0, &headerBuffer[IMAGE_HEADER_OFFSET]);
286 dataProt.resetTransfer();
288 dataProt.setTransferHeader(&headerBuffer[IMAGE_HEADER_OFFSET],
sizeof(HeaderData), numTransferBlocks);
291 int rawDataLength = getFrameSize(imageSet.
getWidth(), imageSet.
getHeight(), 0, 0, 0, bits);
292 dataProt.setTransferBytes(i, rawDataLength);
296 int bits[ImageSet::MAX_SUPPORTED_IMAGES] = {0};
297 int rowSize[ImageSet::MAX_SUPPORTED_IMAGES] = {0};
298 const unsigned char* pixelData[ImageSet::MAX_SUPPORTED_IMAGES] = {
nullptr};
299 std::vector<unsigned char> encodingBuffer[ImageSet::MAX_SUPPORTED_IMAGES];
303 rowSize[i] = imageSet.
getWidth()*bits[i]/8;
308 encodingBuffer[i].resize(rowSize[i] * imageSet.
getHeight());
311 pixelData[i] = &encodingBuffer[i][0];
316 dataProt.setTransferData(i, const_cast<unsigned char*>(pixelData[i]));
320 void ImageProtocol::Pimpl::setRawTransferData(
const ImageSet& metaData,
const std::vector<unsigned char*>& rawData,
321 int firstTileWidth,
int middleTilesWidth,
int lastTileWidth,
int validBytes) {
323 throw ProtocolException(
"Mismatch between metadata and number of image buffers!");
327 copyHeaderToBuffer(metaData, firstTileWidth, middleTilesWidth, lastTileWidth, &headerBuffer[IMAGE_HEADER_OFFSET]);
328 dataProt.resetTransfer();
330 dataProt.setTransferHeader(&headerBuffer[IMAGE_HEADER_OFFSET],
sizeof(HeaderData), numTransferBlocks);
334 firstTileWidth, middleTilesWidth, lastTileWidth, metaData.
getBitsPerPixel(i));
335 dataProt.setTransferBytes(i, rawDataLength);
339 dataProt.setTransferData(i, rawData[i]);
343 void ImageProtocol::Pimpl::setRawValidBytes(
const std::vector<int>& validBytesVec) {
344 for (
int i=0; i<static_cast<int>(validBytesVec.size()); ++i) {
345 dataProt.setTransferValidBytes(i, validBytesVec[i]);
349 const unsigned char* ImageProtocol::Pimpl::getTransferMessage(
int& length) {
350 const unsigned char* msg = dataProt.getTransferMessage(length);
353 msg = dataProt.getTransferMessage(length);
359 bool ImageProtocol::Pimpl::transferComplete() {
360 return dataProt.transferComplete();
363 int ImageProtocol::Pimpl::getNumTiles(
int width,
int firstTileWidth,
int middleTilesWidth,
int lastTileWidth) {
364 if(lastTileWidth == 0) {
366 }
else if(middleTilesWidth == 0) {
369 int tileWidth = firstTileWidth + lastTileWidth - middleTilesWidth;
370 return (width - 2*tileWidth + firstTileWidth + lastTileWidth) / (firstTileWidth + lastTileWidth - tileWidth);
374 int ImageProtocol::Pimpl::getFrameSize(
int width,
int height,
int firstTileWidth,
375 int middleTilesWidth,
int lastTileWidth,
int totalBits) {
376 return (width * height * totalBits) /8;
392 void ImageProtocol::Pimpl::copyHeaderToBuffer(
const ImageSet& imageSet,
393 int firstTileWidth,
int middleTilesWidth,
int lastTileWidth,
unsigned char* buffer) {
394 int timeSec = 0, timeMicrosec = 0;
395 HeaderData* transferHeader =
reinterpret_cast<HeaderData*
>(buffer);
397 memset(transferHeader, 0,
sizeof(*transferHeader));
398 transferHeader->magic = htons(MAGIC_SEQUECE);
399 transferHeader->protocolVersion = InternalInformation::CURRENT_PROTOCOL_VERSION;
400 transferHeader->isRawImagePair_OBSOLETE = 0;
401 transferHeader->width = htons(imageSet.
getWidth());
402 transferHeader->height = htons(imageSet.
getHeight());
403 transferHeader->firstTileWidth = htons(firstTileWidth);
404 transferHeader->lastTileWidth = htons(lastTileWidth);
405 transferHeader->middleTilesWidth = htons(middleTilesWidth);
406 transferHeader->format0 =
static_cast<unsigned char>(imageSet.
getPixelFormat(0));
408 transferHeader->seqNum =
static_cast<unsigned int>(htonl(imageSet.
getSequenceNumber()));
410 transferHeader->numberOfImages =
static_cast<unsigned char>(imageSet.
getNumberOfImages());
414 transferHeader->lastSyncPulseSec = htonl(timeSec);
415 transferHeader->lastSyncPulseMicrosec = htonl(timeMicrosec);
417 transferHeader->totalHeaderSize = htons(
sizeof(HeaderData));
418 transferHeader->flags = htons(HeaderData::FlagBits::NEW_STYLE_TRANSFER | HeaderData::FlagBits::HEADER_V3
419 | HeaderData::FlagBits::HEADER_V4);
421 int minDisp = 0, maxDisp = 0;
423 transferHeader->minDisparity = minDisp;
424 transferHeader->maxDisparity = maxDisp;
429 transferHeader->timeSec =
static_cast<int>(htonl(static_cast<unsigned int>(timeSec)));
430 transferHeader->timeMicrosec =
static_cast<int>(htonl(static_cast<unsigned int>(timeMicrosec)));
432 int numImageChannels = 0;
433 for (
int i=0; i<(int)
sizeof(transferHeader->imageTypes); ++i) {
434 transferHeader->imageTypes[i] =
static_cast<unsigned char>(ImageSet::ImageType::IMAGE_UNDEFINED);
436 int idx = imageSet.
getIndexOf(ImageSet::ImageType::IMAGE_LEFT);
438 transferHeader->imageTypes[idx] =
static_cast<unsigned char>(ImageSet::ImageType::IMAGE_LEFT);
441 idx = imageSet.
getIndexOf(ImageSet::ImageType::IMAGE_RIGHT);
443 transferHeader->imageTypes[idx] =
static_cast<unsigned char>(ImageSet::ImageType::IMAGE_RIGHT);
446 idx = imageSet.
getIndexOf(ImageSet::ImageType::IMAGE_DISPARITY);
448 transferHeader->imageTypes[idx] =
static_cast<unsigned char>(ImageSet::ImageType::IMAGE_DISPARITY);
452 throw std::runtime_error(
"Mismatch between reported number of images and enabled channel selection!");
457 memcpy(transferHeader->q, imageSet.
getQMatrix(),
sizeof(float)*16);
461 void ImageProtocol::Pimpl::resetTransfer() {
462 dataProt.resetTransfer();
465 unsigned char* ImageProtocol::Pimpl::getNextReceiveBuffer(
int& maxLength) {
466 maxLength = dataProt.getMaxReceptionSize();
467 return dataProt.getNextReceiveBuffer(maxLength);
470 void ImageProtocol::Pimpl::processReceivedMessage(
int length) {
471 receptionDone =
false;
474 dataProt.processReceivedMessage(length, receptionDone);
475 if(!dataProt.wasHeaderReceived() && receiveHeaderParsed) {
477 LOG_WARN(
"Resetting image protocol!");
482 int receivedBytes = 0;
483 dataProt.getReceivedData(receivedBytes);
486 if(!receiveHeaderParsed) {
488 unsigned char* headerData = dataProt.getReceivedHeader(headerLen);
489 if(headerData !=
nullptr) {
490 tryDecodeHeader(headerData, headerLen);
495 void ImageProtocol::Pimpl::tryDecodeHeader(
const 496 unsigned char* receivedData,
int receivedBytes) {
499 constexpr
int optionalDataSize =
sizeof(receiveHeader.middleTilesWidth);
500 constexpr
int mandatoryDataSize =
static_cast<int>(
sizeof(HeaderDataLegacy)) - optionalDataSize;
501 constexpr
int fullyExtensibleHeaderSize =
static_cast<int>(
sizeof(HeaderDataV2));
502 bool isCompleteHeader =
false;
504 if(receivedBytes >= mandatoryDataSize) {
505 if (receivedBytes < fullyExtensibleHeaderSize) {
506 *(
static_cast<HeaderDataLegacy*
>(&receiveHeader)) = *
reinterpret_cast<const HeaderDataLegacy*
>(receivedData);
508 memcpy(&receiveHeader, receivedData, std::min((
size_t)receivedBytes,
sizeof(HeaderData)));
509 receiveHeader = *
reinterpret_cast<const HeaderData*
>(receivedData);
510 isCompleteHeader =
true;
512 if(receiveHeader.magic != htons(MAGIC_SEQUECE)) {
518 if(receiveHeader.protocolVersion != InternalInformation::CURRENT_PROTOCOL_VERSION) {
523 receiveHeader.width = ntohs(receiveHeader.width);
524 receiveHeader.height = ntohs(receiveHeader.height);
525 receiveHeader.firstTileWidth = ntohs(receiveHeader.firstTileWidth);
526 receiveHeader.lastTileWidth = ntohs(receiveHeader.lastTileWidth);
528 receiveHeader.timeSec =
static_cast<int>(
529 ntohl(static_cast<unsigned int>(receiveHeader.timeSec)));
530 receiveHeader.timeMicrosec =
static_cast<int>(
531 ntohl(static_cast<unsigned int>(receiveHeader.timeMicrosec)));
532 receiveHeader.seqNum = ntohl(receiveHeader.seqNum);
535 if(receivedBytes >= mandatoryDataSize + optionalDataSize) {
536 receiveHeader.middleTilesWidth = ntohs(receiveHeader.middleTilesWidth);
538 receiveHeader.middleTilesWidth = 0;
540 if (isCompleteHeader) {
542 receiveHeader.totalHeaderSize = ntohs(receiveHeader.totalHeaderSize);
543 receiveHeader.flags = ntohs(receiveHeader.flags);
544 receiveHeader.exposureTime = ntohl(receiveHeader.exposureTime);
545 receiveHeader.lastSyncPulseSec = htonl(receiveHeader.lastSyncPulseSec);
546 receiveHeader.lastSyncPulseMicrosec = htonl(receiveHeader.lastSyncPulseMicrosec);
549 receiveHeader.totalHeaderSize = (receivedBytes <= mandatoryDataSize) ? mandatoryDataSize : static_cast<int>(
sizeof(HeaderDataLegacy));
550 receiveHeader.flags = 0;
551 receiveHeader.numberOfImages = 2;
552 receiveHeader.format2 = 0;
553 receiveHeader.exposureTime = 0;
554 receiveHeader.lastSyncPulseSec = 0;
555 receiveHeader.lastSyncPulseMicrosec = 0;
558 receiveHeaderParsed =
true;
562 bool ImageProtocol::Pimpl::imagesReceived()
const {
563 return receptionDone && receiveHeaderParsed;
566 bool ImageProtocol::Pimpl::getReceivedImageSet(
ImageSet& imageSet) {
567 bool complete =
false;
571 return (ok && complete);
574 bool ImageProtocol::Pimpl::getPartiallyReceivedImageSet(
ImageSet& imageSet,
int& validRows,
bool& complete) {
580 if(!receiveHeaderParsed) {
586 bool flaggedDisparityPair = (receiveHeader.isRawImagePair_OBSOLETE == 0);
587 bool isInterleaved = (receiveHeader.flags & HeaderData::FlagBits::NEW_STYLE_TRANSFER) == 0;
588 bool arbitraryChannels = (receiveHeader.flags & HeaderData::FlagBits::HEADER_V3) > 0;
589 bool hasExposureTime = (receiveHeader.flags & HeaderData::FlagBits::HEADER_V4) > 0;
592 unsigned short unaccountedFlags = receiveHeader.flags & ~(HeaderData::FlagBits::NEW_STYLE_TRANSFER
593 | HeaderData::FlagBits::HEADER_V3 | HeaderData::FlagBits::HEADER_V4);
594 if (unaccountedFlags != 0) {
597 static bool warnedOnceForward =
false;
598 if (!warnedOnceForward) {
599 LOG_WARN(
"Warning: forward-compatible mode; will attempt to process image stream with unknown extra flags. Consider upgrading the client software.");
600 warnedOnceForward =
true;
604 imageSet.
setWidth(receiveHeader.width);
605 imageSet.
setHeight(receiveHeader.height);
606 imageSet.
setPixelFormat(0, static_cast<ImageSet::ImageFormat>(receiveHeader.format0));
610 int rowStrideArr[ImageSet::MAX_SUPPORTED_IMAGES] = {0};
611 int validRowsArr[ImageSet::MAX_SUPPORTED_IMAGES] = {0};
612 unsigned char* pixelArr[ImageSet::MAX_SUPPORTED_IMAGES] = {
nullptr};
616 static bool warnedOnceBackward =
false;
617 if (!warnedOnceBackward) {
618 LOG_WARN(
"Info: backward-compatible mode; the device is sending with a legacy protocol. Consider upgrading its firmware.");
619 warnedOnceBackward =
true;
621 unsigned char* data = dataProt.getBlockReceiveBuffer(0);
622 int validBytes = dataProt.getBlockValidSize(0);
623 for (
int i=0; i < 2; ++i) {
624 pixelArr[i] = decodeInterleaved(i, imageSet.
getNumberOfImages(), validBytes, data, validRowsArr[i], rowStrideArr[i]);
627 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_LEFT, 0);
628 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_RIGHT, flaggedDisparityPair ? -1 : 1);
629 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_DISPARITY, flaggedDisparityPair ? 1 : -1);
633 for (
int i=0; i<receiveHeader.numberOfImages; ++i) {
634 unsigned char* data = dataProt.getBlockReceiveBuffer(i);
635 int validBytes = dataProt.getBlockValidSize(i);
636 pixelArr[i] = decodeNoninterleaved(i, imageSet.
getNumberOfImages(), validBytes, data, validRowsArr[i], rowStrideArr[i]);
639 LOG_WARN(
"Protocol exception: " + ex.what());
643 if (arbitraryChannels) {
645 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_LEFT, -1);
646 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_RIGHT, -1);
647 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_DISPARITY, -1);
649 int typ = receiveHeader.imageTypes[i];
654 static bool warnedOnceV2 =
false;
656 LOG_WARN(
"Info: received a transfer with header v2");
661 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_LEFT, 0);
662 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_RIGHT, flaggedDisparityPair ? -1 : 1);
663 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_DISPARITY, flaggedDisparityPair ? 1 : -1);
665 if(hasExposureTime) {
667 imageSet.
setLastSyncPulse(receiveHeader.lastSyncPulseSec, receiveHeader.lastSyncPulseMicrosec);
671 for (
int i=0; i<receiveHeader.numberOfImages; ++i) {
678 imageSet.
setTimestamp(receiveHeader.timeSec, receiveHeader.timeMicrosec);
679 imageSet.
setDisparityRange(receiveHeader.minDisparity, receiveHeader.maxDisparity);
682 validRows = validRowsArr[0];
683 for (
int i=0; i<receiveHeader.numberOfImages; ++i) {
684 if (validRowsArr[i] < validRows) {
685 validRows = validRowsArr[i];
689 if(validRows == receiveHeader.height || receptionDone) {
698 unsigned char* ImageProtocol::Pimpl::decodeNoninterleaved(
int imageNumber,
int numImages,
int receivedBytes,
699 unsigned char* data,
int& validRows,
int& rowStride) {
702 switch (imageNumber) {
716 throw ProtocolException(
"Not implemented: decodeNoninterleaved with image index > 2");
718 bits = getFormatBits(static_cast<ImageSet::ImageFormat>(format),
false);
720 int totalBits = bits;
721 unsigned char* ret =
nullptr;
723 if(receiveHeader.lastTileWidth == 0) {
724 int bufferOffset0 = 0;
725 int bufferRowStride = receiveHeader.width*(totalBits) / 8;
730 ret = &data[bufferOffset0];
731 rowStride = bufferRowStride;
732 validRows = receivedBytes / bufferRowStride;
735 allocateDecodeBuffer(imageNumber);
736 validRows = receivedBytes / bufferRowStride;
737 rowStride = 2*receiveHeader.width;
738 int lastRow = lastReceivedPayloadBytes[imageNumber] / bufferRowStride;
740 BitConversions::decode12BitPacked(lastRow, validRows, &data[bufferOffset0],
741 &decodeBuffer[imageNumber][0], bufferRowStride, rowStride, receiveHeader.width);
743 ret = &decodeBuffer[imageNumber][0];
747 decodeTiledImage(imageNumber,
748 lastReceivedPayloadBytes[imageNumber], receivedBytes, data,
749 receiveHeader.firstTileWidth * (totalBits) / 8,
750 receiveHeader.middleTilesWidth * (totalBits) / 8,
751 receiveHeader.lastTileWidth * (totalBits) / 8,
752 validRows, format,
false);
753 ret = &decodeBuffer[imageNumber][0];
754 rowStride = receiveHeader.width*getFormatBits(
755 static_cast<ImageSet::ImageFormat>(format),
true)/8;
758 lastReceivedPayloadBytes[imageNumber] = receivedBytes;
763 unsigned char* ImageProtocol::Pimpl::decodeInterleaved(
int imageNumber,
int numImages,
int receivedBytes,
764 unsigned char* data,
int& validRows,
int& rowStride) {
766 imageNumber == 0 ? receiveHeader.format0 : receiveHeader.format1);
767 int bits0 = getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format0),
false);
768 int bits1 = getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format1),
false);
769 int bits2 = getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format2),
false);
771 int totalBits = (numImages<3)?(bits0 + bits1):(bits0 + bits1 + bits2);
773 unsigned char* ret =
nullptr;
775 if(receiveHeader.lastTileWidth == 0) {
777 switch (imageNumber) {
778 case 0: { bufferOffset = 0;
break; }
779 case 1: { bufferOffset = receiveHeader.width * bits0/8;
break; }
780 case 2: { bufferOffset = receiveHeader.width * (bits0 + bits1)/8;
break; }
784 int bufferRowStride = receiveHeader.width*(totalBits) / 8;
789 ret = &data[bufferOffset];
790 rowStride = bufferRowStride;
791 validRows = receivedBytes / bufferRowStride;
794 allocateDecodeBuffer(imageNumber);
795 validRows = std::min(receivedBytes / bufferRowStride, (
int)receiveHeader.height);
796 rowStride = 2*receiveHeader.width;
797 int lastRow = lastReceivedPayloadBytes[imageNumber] / bufferRowStride;
799 BitConversions::decode12BitPacked(lastRow, validRows, &data[bufferOffset],
800 &decodeBuffer[imageNumber][0], bufferRowStride, rowStride, receiveHeader.width);
802 ret = &decodeBuffer[imageNumber][0];
806 decodeTiledImage(imageNumber,
807 lastReceivedPayloadBytes[imageNumber], receivedBytes, data,
808 receiveHeader.firstTileWidth * (totalBits) / 8,
809 receiveHeader.middleTilesWidth * (totalBits) / 8,
810 receiveHeader.lastTileWidth * (totalBits) / 8,
811 validRows, format,
true);
812 ret = &decodeBuffer[imageNumber][0];
813 rowStride = receiveHeader.width*getFormatBits(
814 static_cast<ImageSet::ImageFormat>(format),
true)/8;
817 lastReceivedPayloadBytes[imageNumber] = receivedBytes;
821 void ImageProtocol::Pimpl::allocateDecodeBuffer(
int imageNumber) {
823 switch (imageNumber) {
837 throw ProtocolException(
"Not implemented: allocateDecodeBuffer with image index > 2");
839 int bitsPerPixel = getFormatBits(format,
true);
840 int bufferSize = receiveHeader.width * receiveHeader.height * bitsPerPixel / 8;
842 if(decodeBuffer[imageNumber].size() != static_cast<unsigned int>(bufferSize)) {
843 decodeBuffer[imageNumber].resize(bufferSize);
847 void ImageProtocol::Pimpl::decodeTiledImage(
int imageNumber,
int lastReceivedPayloadBytes,
int receivedPayloadBytes,
848 const unsigned char* data,
int firstTileStride,
int middleTilesStride,
int lastTileStride,
int& validRows,
851 allocateDecodeBuffer(imageNumber);
854 int numTiles = getNumTiles(receiveHeader.width, receiveHeader.firstTileWidth,
855 receiveHeader.middleTilesWidth, receiveHeader.lastTileWidth);
856 int payloadOffset = 0;
857 int decodeXOffset = 0;
858 int prevTileStrides = 0;
859 for(
int i = 0; i < numTiles; i++) {
865 tileStride = firstTileStride;
866 tileWidth = receiveHeader.firstTileWidth;
867 }
else if(i == numTiles-1) {
868 tileStride = lastTileStride;
869 tileWidth = receiveHeader.lastTileWidth;
871 tileStride = middleTilesStride;
872 tileWidth = receiveHeader.middleTilesWidth;
875 int tileStart = std::max(0, (lastReceivedPayloadBytes - payloadOffset) / tileStride);
876 int tileStop = std::min(std::max(0, (receivedPayloadBytes - payloadOffset) / tileStride), (
int)receiveHeader.height);
878 if (dataIsInterleaved) {
879 switch (imageNumber) {
880 case 0: { tileOffset = 0;
break; }
881 case 1: { tileOffset = tileWidth * (
882 getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format0),
false)
884 case 2: { tileOffset = tileWidth * (
885 getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format0),
false)
886 + getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format1),
false)
895 tileOffset += receiveHeader.height * prevTileStrides;
902 BitConversions::decode12BitPacked(tileStart, tileStop, &data[tileOffset],
903 &decodeBuffer[imageNumber][decodeXOffset], tileStride, 2*receiveHeader.width, tileWidth);
906 decodeRowsFromTile(tileStart, tileStop, &data[tileOffset],
907 &decodeBuffer[imageNumber][decodeXOffset], tileStride,
908 receiveHeader.width*bytesPixel, tileWidth*bytesPixel);
911 payloadOffset += receiveHeader.height * tileStride;
912 decodeXOffset += tileWidth * bytesPixel;
913 prevTileStrides += tileStride;
914 if(i == numTiles-1) {
915 validRows = tileStop;
920 void ImageProtocol::Pimpl::decodeRowsFromTile(
int startRow,
int stopRow,
unsigned const char* src,
921 unsigned char* dst,
int srcStride,
int dstStride,
int tileWidth) {
922 for(
int y = startRow; y < stopRow; y++) {
923 memcpy(&dst[y*dstStride], &src[y*srcStride], tileWidth);
927 void ImageProtocol::Pimpl::resetReception() {
928 receiveHeaderParsed =
false;
929 for (
int i=0; i<ImageSet::MAX_SUPPORTED_IMAGES; ++i) {
930 lastReceivedPayloadBytes[i] = 0;
932 dataProt.resetReception(
false);
933 receptionDone =
false;
936 bool ImageProtocol::Pimpl::isConnected()
const {
937 return dataProt.isConnected();
940 const unsigned char* ImageProtocol::Pimpl::getNextControlMessage(
int& length) {
941 return dataProt.getNextControlMessage(length);
944 bool ImageProtocol::Pimpl::newClientConnected() {
945 return dataProt.newClientConnected();
948 int ImageProtocol::Pimpl::getNumDroppedFrames()
const {
949 return dataProt.getDroppedReceptions();
952 std::string ImageProtocol::statusReport() {
953 return pimpl->statusReport();
955 std::string ImageProtocol::Pimpl::statusReport() {
956 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.
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 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.