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);
632 for (
int i=0; i<receiveHeader.numberOfImages; ++i) {
633 unsigned char* data = dataProt.getBlockReceiveBuffer(i);
634 int validBytes = dataProt.getBlockValidSize(i);
635 pixelArr[i] = decodeNoninterleaved(i, imageSet.
getNumberOfImages(), validBytes, data, validRowsArr[i], rowStrideArr[i]);
637 if (arbitraryChannels) {
639 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_LEFT, -1);
640 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_RIGHT, -1);
641 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_DISPARITY, -1);
643 int typ = receiveHeader.imageTypes[i];
648 static bool warnedOnceV2 =
false;
650 LOG_WARN(
"Info: received a transfer with header v2");
655 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_LEFT, 0);
656 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_RIGHT, flaggedDisparityPair ? -1 : 1);
657 imageSet.
setIndexOf(ImageSet::ImageType::IMAGE_DISPARITY, flaggedDisparityPair ? 1 : -1);
659 if(hasExposureTime) {
661 imageSet.
setLastSyncPulse(receiveHeader.lastSyncPulseSec, receiveHeader.lastSyncPulseMicrosec);
665 for (
int i=0; i<receiveHeader.numberOfImages; ++i) {
672 imageSet.
setTimestamp(receiveHeader.timeSec, receiveHeader.timeMicrosec);
673 imageSet.
setDisparityRange(receiveHeader.minDisparity, receiveHeader.maxDisparity);
676 validRows = validRowsArr[0];
677 for (
int i=0; i<receiveHeader.numberOfImages; ++i) {
678 if (validRowsArr[i] < validRows) {
679 validRows = validRowsArr[i];
683 if(validRows == receiveHeader.height || receptionDone) {
692 unsigned char* ImageProtocol::Pimpl::decodeNoninterleaved(
int imageNumber,
int numImages,
int receivedBytes,
693 unsigned char* data,
int& validRows,
int& rowStride) {
696 switch (imageNumber) {
710 throw ProtocolException(
"Not implemented: decodeNoninterleaved with image index > 2");
712 bits = getFormatBits(static_cast<ImageSet::ImageFormat>(format),
false);
714 int totalBits = bits;
715 unsigned char* ret =
nullptr;
717 if(receiveHeader.lastTileWidth == 0) {
718 int bufferOffset0 = 0;
719 int bufferRowStride = receiveHeader.width*(totalBits) / 8;
724 ret = &data[bufferOffset0];
725 rowStride = bufferRowStride;
726 validRows = receivedBytes / bufferRowStride;
729 allocateDecodeBuffer(imageNumber);
730 validRows = receivedBytes / bufferRowStride;
731 rowStride = 2*receiveHeader.width;
732 int lastRow = lastReceivedPayloadBytes[imageNumber] / bufferRowStride;
734 BitConversions::decode12BitPacked(lastRow, validRows, &data[bufferOffset0],
735 &decodeBuffer[imageNumber][0], bufferRowStride, rowStride, receiveHeader.width);
737 ret = &decodeBuffer[imageNumber][0];
741 decodeTiledImage(imageNumber,
742 lastReceivedPayloadBytes[imageNumber], receivedBytes, data,
743 receiveHeader.firstTileWidth * (totalBits) / 8,
744 receiveHeader.middleTilesWidth * (totalBits) / 8,
745 receiveHeader.lastTileWidth * (totalBits) / 8,
746 validRows, format,
false);
747 ret = &decodeBuffer[imageNumber][0];
748 rowStride = receiveHeader.width*getFormatBits(
749 static_cast<ImageSet::ImageFormat>(format),
true)/8;
752 lastReceivedPayloadBytes[imageNumber] = receivedBytes;
757 unsigned char* ImageProtocol::Pimpl::decodeInterleaved(
int imageNumber,
int numImages,
int receivedBytes,
758 unsigned char* data,
int& validRows,
int& rowStride) {
760 imageNumber == 0 ? receiveHeader.format0 : receiveHeader.format1);
761 int bits0 = getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format0),
false);
762 int bits1 = getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format1),
false);
763 int bits2 = getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format2),
false);
765 int totalBits = (numImages<3)?(bits0 + bits1):(bits0 + bits1 + bits2);
767 unsigned char* ret =
nullptr;
769 if(receiveHeader.lastTileWidth == 0) {
771 switch (imageNumber) {
772 case 0: { bufferOffset = 0;
break; }
773 case 1: { bufferOffset = receiveHeader.width * bits0/8;
break; }
774 case 2: { bufferOffset = receiveHeader.width * (bits0 + bits1)/8;
break; }
778 int bufferRowStride = receiveHeader.width*(totalBits) / 8;
783 ret = &data[bufferOffset];
784 rowStride = bufferRowStride;
785 validRows = receivedBytes / bufferRowStride;
788 allocateDecodeBuffer(imageNumber);
789 validRows = receivedBytes / bufferRowStride;
790 rowStride = 2*receiveHeader.width;
791 int lastRow = lastReceivedPayloadBytes[imageNumber] / bufferRowStride;
793 BitConversions::decode12BitPacked(lastRow, validRows, &data[bufferOffset],
794 &decodeBuffer[imageNumber][0], bufferRowStride, rowStride, receiveHeader.width);
796 ret = &decodeBuffer[imageNumber][0];
800 decodeTiledImage(imageNumber,
801 lastReceivedPayloadBytes[imageNumber], receivedBytes, data,
802 receiveHeader.firstTileWidth * (totalBits) / 8,
803 receiveHeader.middleTilesWidth * (totalBits) / 8,
804 receiveHeader.lastTileWidth * (totalBits) / 8,
805 validRows, format,
true);
806 ret = &decodeBuffer[imageNumber][0];
807 rowStride = receiveHeader.width*getFormatBits(
808 static_cast<ImageSet::ImageFormat>(format),
true)/8;
811 lastReceivedPayloadBytes[imageNumber] = receivedBytes;
815 void ImageProtocol::Pimpl::allocateDecodeBuffer(
int imageNumber) {
817 switch (imageNumber) {
831 throw ProtocolException(
"Not implemented: allocateDecodeBuffer with image index > 2");
833 int bitsPerPixel = getFormatBits(format,
true);
834 int bufferSize = receiveHeader.width * receiveHeader.height * bitsPerPixel / 8;
836 if(decodeBuffer[imageNumber].size() != static_cast<unsigned int>(bufferSize)) {
837 decodeBuffer[imageNumber].resize(bufferSize);
841 void ImageProtocol::Pimpl::decodeTiledImage(
int imageNumber,
int lastReceivedPayloadBytes,
int receivedPayloadBytes,
842 const unsigned char* data,
int firstTileStride,
int middleTilesStride,
int lastTileStride,
int& validRows,
845 allocateDecodeBuffer(imageNumber);
848 int numTiles = getNumTiles(receiveHeader.width, receiveHeader.firstTileWidth,
849 receiveHeader.middleTilesWidth, receiveHeader.lastTileWidth);
850 int payloadOffset = 0;
851 int decodeXOffset = 0;
852 int prevTileStrides = 0;
853 for(
int i = 0; i < numTiles; i++) {
859 tileStride = firstTileStride;
860 tileWidth = receiveHeader.firstTileWidth;
861 }
else if(i == numTiles-1) {
862 tileStride = lastTileStride;
863 tileWidth = receiveHeader.lastTileWidth;
865 tileStride = middleTilesStride;
866 tileWidth = receiveHeader.middleTilesWidth;
869 int tileStart = std::max(0, (lastReceivedPayloadBytes - payloadOffset) / tileStride);
870 int tileStop = std::min(std::max(0, (receivedPayloadBytes - payloadOffset) / tileStride), (
int)receiveHeader.height);
872 if (dataIsInterleaved) {
873 switch (imageNumber) {
874 case 0: { tileOffset = 0;
break; }
875 case 1: { tileOffset = tileWidth * (
876 getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format0),
false)
878 case 2: { tileOffset = tileWidth * (
879 getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format0),
false)
880 + getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format1),
false)
889 tileOffset += receiveHeader.height * prevTileStrides;
896 BitConversions::decode12BitPacked(tileStart, tileStop, &data[tileOffset],
897 &decodeBuffer[imageNumber][decodeXOffset], tileStride, 2*receiveHeader.width, tileWidth);
900 decodeRowsFromTile(tileStart, tileStop, &data[tileOffset],
901 &decodeBuffer[imageNumber][decodeXOffset], tileStride,
902 receiveHeader.width*bytesPixel, tileWidth*bytesPixel);
905 payloadOffset += receiveHeader.height * tileStride;
906 decodeXOffset += tileWidth * bytesPixel;
907 prevTileStrides += tileStride;
908 if(i == numTiles-1) {
909 validRows = tileStop;
914 void ImageProtocol::Pimpl::decodeRowsFromTile(
int startRow,
int stopRow,
unsigned const char* src,
915 unsigned char* dst,
int srcStride,
int dstStride,
int tileWidth) {
916 for(
int y = startRow; y < stopRow; y++) {
917 memcpy(&dst[y*dstStride], &src[y*srcStride], tileWidth);
921 void ImageProtocol::Pimpl::resetReception() {
922 receiveHeaderParsed =
false;
923 for (
int i=0; i<ImageSet::MAX_SUPPORTED_IMAGES; ++i) {
924 lastReceivedPayloadBytes[i] = 0;
926 dataProt.resetReception(
false);
927 receptionDone =
false;
930 bool ImageProtocol::Pimpl::isConnected()
const {
931 return dataProt.isConnected();
934 const unsigned char* ImageProtocol::Pimpl::getNextControlMessage(
int& length) {
935 return dataProt.getNextControlMessage(length);
938 bool ImageProtocol::Pimpl::newClientConnected() {
939 return dataProt.newClientConnected();
942 int ImageProtocol::Pimpl::getNumDroppedFrames()
const {
943 return dataProt.getDroppedReceptions();
946 std::string ImageProtocol::statusReport() {
947 return pimpl->statusReport();
949 std::string ImageProtocol::Pimpl::statusReport() {
950 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.