21 #include "visiontransfer/imageprotocol.h" 22 #include "visiontransfer/alignedallocator.h" 23 #include "visiontransfer/datablockprotocol.h" 24 #include "visiontransfer/exceptions.h" 33 #include <arpa/inet.h> 38 #include <immintrin.h> 40 #include <emmintrin.h> 47 class ImageProtocol::Pimpl {
49 Pimpl(ProtocolType protType);
52 void setTransferImagePair(
const ImagePair& imagePair);
53 void setRawTransferData(
const ImagePair& metaData,
unsigned char* rawData,
54 int firstTileWidth = 0,
int secondTileWidth = 0,
int validBytes = 0x7FFFFFFF);
55 void setRawValidBytes(
int validBytes);
56 const unsigned char* getTransferMessage(
int& length);
57 bool transferComplete();
59 bool getReceivedImagePair(
ImagePair& imagePair);
60 bool getPartiallyReceivedImagePair(
ImagePair& imagePair,
61 int& validRows,
bool& complete);
62 bool imagesReceived()
const;
64 unsigned char* getNextReceiveBuffer(
int& maxLength);
66 bool processReceivedMessage(
int length);
67 int getProspectiveMessageSize();
68 void resetReception();
74 unsigned char protocolVersion;
75 unsigned char padding0;
78 unsigned short height;
80 unsigned short firstTileWidth;
81 unsigned short secondTileWidth;
83 unsigned char format0;
84 unsigned char format1;
85 unsigned char minDisparity;
86 unsigned char maxDisparity;
94 unsigned char padding1[6];
98 static const unsigned char CURRENT_VERSION = 0x04;
102 ProtocolType protType;
105 bool headerTransferred;
106 std::vector<unsigned char> headerBuffer;
107 std::vector<unsigned char> rawBuffer;
108 unsigned char* rawData;
113 std::vector<unsigned char, AlignedAllocator<unsigned char> >decodeBuffer[2];
114 bool receiveHeaderParsed;
115 HeaderData receiveHeader;
116 int lastReceivedPayloadBytes[2];
117 int receiveTotalSize;
121 void copyHeaderToBuffer(
const ImagePair& imagePair,
int firstTileWidth,
122 int secondTileWidth,
unsigned char* buffer);
125 void tryDecodeHeader(
const unsigned char* receivedData,
int receivedBytes);
128 unsigned char* decodeInterleaved(
int imageNumber,
int receivedBytes,
129 unsigned char* data,
int& validRows,
int& rowStride);
132 void decodeSubpixel(
int startRow,
int stopRow,
unsigned const char* src,
133 unsigned char* dst,
int srcStride,
int dstStride,
int rowWidth);
136 template <
bool alignedLoad>
137 void decodeSubpixelSSE2(
int startRow,
int stopRow,
const unsigned char* dispStart,
138 const unsigned char* subpixStart,
int width,
unsigned short* dst,
int srcStride,
int dstStride);
140 template <
bool alignedLoad>
141 void decodeSubpixelAVX2(
int startRow,
int stopRow,
const unsigned char* dispStart,
142 const unsigned char* subpixStart,
int width,
unsigned short* dst,
int srcStride,
int dstStride);
144 void decodeSubpixelFallback(
int startRow,
int stopRow,
const unsigned char* dispStart,
145 const unsigned char* subpixStart,
int width,
unsigned short* dst,
int srcStride,
int dstStride);
147 int getFrameSize(
int width,
int height,
int firstTileWidth,
int secondTileWidth,
152 void decodeTiledImage(
int imageNumber,
int lastReceivedPayloadBytes,
int receivedPayloadBytes,
153 const unsigned char* data,
int firstTileStride,
int secondTileStride,
int& validRows,
156 void decodeRowsFromTile(
int startRow,
int stopRow,
unsigned const char* src,
157 unsigned char* dst,
int srcStride,
int dstStride,
int tileWidth);
159 void allocateDecodeBuffer(
int imageNumber);
166 : pimpl(new Pimpl(protType)) {
170 ImageProtocol::~ImageProtocol() {
175 pimpl->setTransferImagePair(imagePair);
179 unsigned char* imageData,
int firstTileWidth,
int secondTileWidth,
int validBytes) {
180 pimpl->setRawTransferData(metaData, imageData, firstTileWidth, secondTileWidth, validBytes);
184 pimpl->setRawValidBytes(validBytes);
188 return pimpl->getTransferMessage(length);
192 return pimpl->transferComplete();
196 pimpl->resetTransfer();
200 return pimpl->getReceivedImagePair(imagePair);
204 ImagePair& imagePair,
int& validRows,
bool& complete) {
205 return pimpl->getPartiallyReceivedImagePair(imagePair, validRows, complete);
209 return pimpl->imagesReceived();
213 return pimpl->getNextReceiveBuffer(maxLength);
217 return pimpl->processReceivedMessage(length);
221 pimpl->resetReception();
227 :dataProt(static_cast<DataBlockProtocol::ProtocolType>(protType)),
228 protType(protType), headerTransferred(
false),
229 rawData(
nullptr), rawValidBytes(0), rawDataLength(0), receiveHeaderParsed(
false),
230 lastReceivedPayloadBytes{0, 0}, receiveTotalSize(0), receptionDone(
false) {
231 headerBuffer.resize(
sizeof(HeaderData) +
sizeof(
unsigned short));
232 memset(&headerBuffer[0], 0,
sizeof(headerBuffer.size()));
233 memset(&receiveHeader, 0,
sizeof(receiveHeader));
237 dataProt.setReceiveDataSize(DataBlockProtocol::MAX_UDP_BYTES_TRANSFER);
240 void ImageProtocol::Pimpl::setTransferImagePair(
const ImagePair& imagePair) {
245 headerTransferred =
false;
248 copyHeaderToBuffer(imagePair, 0, 0, &headerBuffer[0]);
249 dataProt.startTransfer();
250 dataProt.setTransferData(&headerBuffer[0],
sizeof(HeaderData));
256 rawBuffer.resize(imagePair.
getWidth()*imagePair.
getHeight()*(bytes0 + bytes1) +
sizeof(
short));
258 int bufferOffset = 0;
259 int row0Size = imagePair.
getWidth()*bytes0;
260 int row1Size = imagePair.
getWidth()*bytes1;
261 for(
int y = 0; y<imagePair.
getHeight(); y++) {
263 bufferOffset += row0Size;
266 bufferOffset += row1Size;
269 rawData = &rawBuffer[0];
270 rawValidBytes =
static_cast<int>(rawBuffer.size() -
sizeof(short));
276 void ImageProtocol::Pimpl::setRawTransferData(
const ImagePair& metaData,
unsigned char* rawData,
277 int firstTileWidth,
int secondTileWidth,
int validBytes) {
278 if(rawData ==
nullptr) {
284 headerTransferred =
false;
287 copyHeaderToBuffer(metaData, firstTileWidth, secondTileWidth, &headerBuffer[0]);
288 dataProt.startTransfer();
289 dataProt.setTransferData(&headerBuffer[0],
sizeof(HeaderData));
291 this->rawData = rawData;
292 rawValidBytes = validBytes;
299 void ImageProtocol::Pimpl::setRawValidBytes(
int validBytes) {
300 rawValidBytes = validBytes;
301 if(headerTransferred) {
302 dataProt.setTransferValidBytes(validBytes);
306 const unsigned char* ImageProtocol::Pimpl::getTransferMessage(
int& length) {
307 const unsigned char* msg = dataProt.getTransferMessage(length);
309 if(msg ==
nullptr && !headerTransferred && rawValidBytes > 0) {
312 headerTransferred =
true;
313 dataProt.setTransferData(rawData, rawDataLength, rawValidBytes);
314 msg = dataProt.getTransferMessage(length);
320 bool ImageProtocol::Pimpl::transferComplete() {
321 return dataProt.transferComplete() && headerTransferred;
324 int ImageProtocol::Pimpl::getFrameSize(
int width,
int height,
int firstTileWidth,
330 int effectiveWidth = firstTileWidth > 0 ? firstTileWidth + secondTileWidth : width;
332 return (effectiveWidth * height * (nibbles0 + nibbles1)) /2 + headerSize;
344 void ImageProtocol::Pimpl::copyHeaderToBuffer(
const ImagePair& imagePair,
345 int firstTileWidth,
int secondTileWidth,
unsigned char* buffer) {
346 HeaderData* transferHeader =
reinterpret_cast<HeaderData*
>(buffer);
347 memset(transferHeader, 0,
sizeof(*transferHeader));
348 transferHeader->protocolVersion = CURRENT_VERSION;
349 transferHeader->width = htons(imagePair.
getWidth());
350 transferHeader->height = htons(imagePair.
getHeight());
351 transferHeader->firstTileWidth = htons(firstTileWidth);
352 transferHeader->secondTileWidth = htons(secondTileWidth);
353 transferHeader->format0 =
static_cast<unsigned char>(imagePair.
getPixelFormat(0));
354 transferHeader->format1 =
static_cast<unsigned char>(imagePair.
getPixelFormat(1));
355 transferHeader->seqNum =
static_cast<unsigned int>(htonl(imagePair.
getSequenceNumber()));
357 int minDisp = 0, maxDisp = 0;
359 transferHeader->minDisparity = minDisp;
360 transferHeader->maxDisparity = maxDisp;
362 int timeSec = 0, timeMicrosec = 0;
364 transferHeader->timeSec =
static_cast<int>(htonl(static_cast<unsigned int>(timeSec)));
365 transferHeader->timeMicrosec =
static_cast<int>(htonl(static_cast<unsigned int>(timeMicrosec)));
368 memcpy(transferHeader->q, imagePair.
getQMatrix(),
sizeof(float)*16);
372 void ImageProtocol::Pimpl::resetTransfer() {
373 dataProt.resetTransfer();
376 unsigned char* ImageProtocol::Pimpl::getNextReceiveBuffer(
int& maxLength) {
377 maxLength = dataProt.getMaxPayloadSize() + dataProt.getProtocolOverhead();
378 return dataProt.getNextReceiveBuffer(maxLength);
381 bool ImageProtocol::Pimpl::processReceivedMessage(
int length) {
382 receptionDone =
false;
385 if(!dataProt.processReceivedMessage(length)) {
390 int receivedBytes = 0;
391 const unsigned char* receivedData = dataProt.getReceivedData(receivedBytes);
394 if(!receiveHeaderParsed && receivedBytes > 0) {
395 tryDecodeHeader(receivedData, receivedBytes);
399 if(receivedBytes == receiveTotalSize) {
400 receptionDone =
true;
401 dataProt.finishReception();
402 }
else if(receivedBytes > receiveTotalSize) {
404 dataProt.resetReception();
411 void ImageProtocol::Pimpl::tryDecodeHeader(
const 412 unsigned char* receivedData,
int receivedBytes) {
413 if(receivedBytes >= static_cast<int>(
sizeof(HeaderData))) {
414 receiveHeader = *
reinterpret_cast<const HeaderData*
>(receivedData);
416 if(receiveHeader.protocolVersion > CURRENT_VERSION ||
417 receiveHeader.protocolVersion < 4) {
422 receiveHeader.width = ntohs(receiveHeader.width);
423 receiveHeader.height = ntohs(receiveHeader.height);
424 receiveHeader.firstTileWidth = ntohs(receiveHeader.firstTileWidth);
425 receiveHeader.secondTileWidth = ntohs(receiveHeader.secondTileWidth);
426 receiveHeader.timeSec =
static_cast<int>(
427 htonl(static_cast<unsigned int>(receiveHeader.timeSec)));
428 receiveHeader.timeMicrosec =
static_cast<int>(
429 htonl(static_cast<unsigned int>(receiveHeader.timeMicrosec)));
430 receiveHeader.seqNum = htonl(receiveHeader.seqNum);
433 receiveTotalSize = getFrameSize(
435 receiveHeader.height,
436 receiveHeader.firstTileWidth,
437 receiveHeader.secondTileWidth,
438 static_cast<ImagePair::ImageFormat>(receiveHeader.format0),
439 static_cast<ImagePair::ImageFormat>(receiveHeader.format1),
442 dataProt.setReceiveDataSize(receiveTotalSize);
443 receiveHeaderParsed =
true;
447 bool ImageProtocol::Pimpl::imagesReceived()
const {
448 return receptionDone && receiveHeaderParsed;
451 bool ImageProtocol::Pimpl::getReceivedImagePair(
ImagePair& imagePair) {
452 bool complete =
false;
456 return (ok && complete);
459 bool ImageProtocol::Pimpl::getPartiallyReceivedImagePair(
ImagePair& imagePair,
int& validRows,
bool& complete) {
465 if(!receiveHeaderParsed) {
470 int receivedBytes = 0;
471 unsigned char* data = dataProt.getReceivedData(receivedBytes);
472 if(receivedBytes == receiveTotalSize) {
474 dataProt.finishReception();
478 imagePair.
setWidth(receiveHeader.width);
479 imagePair.
setHeight(receiveHeader.height);
480 imagePair.
setPixelFormat(0, static_cast<ImagePair::ImageFormat>(receiveHeader.format0));
481 imagePair.
setPixelFormat(1, static_cast<ImagePair::ImageFormat>(receiveHeader.format1));
483 int rowStride0 = 0, rowStride1 = 0;
484 int validRows0 = 0, validRows1 = 0;
485 unsigned char* pixel0 = decodeInterleaved(0, receivedBytes, data, validRows0, rowStride0);
486 unsigned char* pixel1 = decodeInterleaved(1, receivedBytes, data, validRows1, rowStride1);
495 imagePair.
setTimestamp(receiveHeader.timeSec, receiveHeader.timeMicrosec);
496 imagePair.
setDisparityRange(receiveHeader.minDisparity, receiveHeader.maxDisparity);
498 validRows = min(validRows0, validRows1);
500 if(validRows == receiveHeader.height) {
513 unsigned char* ImageProtocol::Pimpl::decodeInterleaved(
int imageNumber,
int receivedBytes,
514 unsigned char* data,
int& validRows,
int& rowStride) {
515 if(receivedBytes <= static_cast<int>(
sizeof(HeaderData))) {
521 imageNumber == 0 ? receiveHeader.format0 : receiveHeader.format1);
522 int nibbles0 = getFormatNibbles(static_cast<ImagePair::ImageFormat>(receiveHeader.format0));
523 int nibbles1 = getFormatNibbles(static_cast<ImagePair::ImageFormat>(receiveHeader.format1));
525 unsigned char* ret =
nullptr;
526 int payloadBytes = receivedBytes -
sizeof(HeaderData);
528 if(receiveHeader.secondTileWidth == 0) {
529 int bufferOffset =
sizeof(HeaderData) + imageNumber*receiveHeader.width * nibbles0/2;
530 int bufferRowStride = receiveHeader.width*(nibbles0 + nibbles1) / 2;
534 allocateDecodeBuffer(imageNumber);
535 validRows = payloadBytes / bufferRowStride;
536 rowStride = 2*receiveHeader.width;
537 int lastRow = lastReceivedPayloadBytes[imageNumber] / bufferRowStride;
539 decodeSubpixel(lastRow, validRows, &data[bufferOffset],
540 &decodeBuffer[imageNumber][0], bufferRowStride, rowStride, receiveHeader.width);
541 ret = &decodeBuffer[imageNumber][0];
545 ret = &data[bufferOffset];
546 rowStride = bufferRowStride;
547 validRows = payloadBytes / bufferRowStride;
551 decodeTiledImage(imageNumber,
552 lastReceivedPayloadBytes[imageNumber], payloadBytes,
553 data, receiveHeader.firstTileWidth * (nibbles0 + nibbles1) / 2,
554 receiveHeader.secondTileWidth * (nibbles0 + nibbles1) / 2,
556 ret = &decodeBuffer[imageNumber][0];
559 rowStride = 2*receiveHeader.width;
561 rowStride = receiveHeader.width;
565 lastReceivedPayloadBytes[imageNumber] = payloadBytes;
569 void ImageProtocol::Pimpl::allocateDecodeBuffer(
int imageNumber) {
571 imageNumber == 0 ? receiveHeader.format0 : receiveHeader.format1);
573 int bufferSize = receiveHeader.width * receiveHeader.height * bytesPerPixel;
575 if(decodeBuffer[imageNumber].size() !=
static_cast<unsigned int>(bufferSize)) {
576 decodeBuffer[imageNumber].resize(bufferSize);
580 void ImageProtocol::Pimpl::decodeTiledImage(
int imageNumber,
int lastReceivedPayloadBytes,
int receivedPayloadBytes,
581 const unsigned char* data,
int firstTileStride,
int secondTileStride,
int& validRows,
585 allocateDecodeBuffer(imageNumber);
588 int startFirstTile = lastReceivedPayloadBytes / firstTileStride;
589 int stopFirstTile = std::min(receivedPayloadBytes / firstTileStride,
590 static_cast<int>(receiveHeader.height));
593 int secondTileBytes = receivedPayloadBytes - (receiveHeader.height*firstTileStride);
594 int lastSecondTileBytes = lastReceivedPayloadBytes - (receiveHeader.height*firstTileStride);
595 int startSecondTile = std::max(0, lastSecondTileBytes / secondTileStride);
596 int stopSecondTile = std::max(0, secondTileBytes / secondTileStride);
597 int firstTileOffset =
sizeof(HeaderData) + imageNumber * getFormatNibbles(
598 static_cast<ImagePair::ImageFormat>(receiveHeader.format0)) * receiveHeader.firstTileWidth / 2;
602 decodeSubpixel(startFirstTile, stopFirstTile, &data[firstTileOffset], &decodeBuffer[imageNumber][0],
603 firstTileStride, 2*receiveHeader.width, receiveHeader.firstTileWidth);
605 decodeRowsFromTile(startFirstTile, stopFirstTile, &data[firstTileOffset],
606 &decodeBuffer[imageNumber][0], firstTileStride, receiveHeader.width,
607 receiveHeader.firstTileWidth);
611 int secondTileOffset =
sizeof(HeaderData) + receiveHeader.height*firstTileStride +
612 imageNumber * getFormatNibbles(static_cast<ImagePair::ImageFormat>(receiveHeader.format0)) * receiveHeader.secondTileWidth / 2;
615 decodeSubpixel(startSecondTile, stopSecondTile,
616 &data[secondTileOffset], &decodeBuffer[imageNumber][2*receiveHeader.firstTileWidth],
617 secondTileStride, 2*receiveHeader.width, receiveHeader.secondTileWidth);
619 decodeRowsFromTile(startSecondTile, stopSecondTile, &data[secondTileOffset],
620 &decodeBuffer[imageNumber][receiveHeader.firstTileWidth],
621 secondTileStride, receiveHeader.width, receiveHeader.secondTileWidth);
624 validRows = stopSecondTile;
627 void ImageProtocol::Pimpl::decodeRowsFromTile(
int startRow,
int stopRow,
unsigned const char* src,
628 unsigned char* dst,
int srcStride,
int dstStride,
int tileWidth) {
629 for(
int y = startRow; y < stopRow; y++) {
630 memcpy(&dst[y*dstStride], &src[y*srcStride], tileWidth);
634 void ImageProtocol::Pimpl::resetReception() {
635 receiveHeaderParsed =
false;
636 lastReceivedPayloadBytes[0] = 0;
637 lastReceivedPayloadBytes[1] = 0;
638 receiveTotalSize = 0;
639 dataProt.resetReception();
640 receptionDone =
false;
643 void ImageProtocol::Pimpl::decodeSubpixel(
int startRow,
int stopRow,
unsigned const char* src,
644 unsigned char* dst,
int srcStride,
int dstStride,
int rowWidth) {
646 const unsigned char* dispStart = src;
647 const unsigned char* subpixStart = &src[rowWidth];
650 if(rowWidth % 32 == 0) {
651 if(srcStride % 32 == 0 && reinterpret_cast<size_t>(src) % 32 == 0) {
652 decodeSubpixelAVX2<true>(startRow, stopRow, dispStart, subpixStart,
653 rowWidth,
reinterpret_cast<unsigned short*
>(dst), srcStride, dstStride);
655 decodeSubpixelAVX2<false>(startRow, stopRow, dispStart, subpixStart,
656 rowWidth,
reinterpret_cast<unsigned short*
>(dst), srcStride, dstStride);
662 if(rowWidth % 16 == 0) {
663 if(srcStride % 16 == 0 && reinterpret_cast<size_t>(src) % 16 == 0) {
664 decodeSubpixelSSE2<true>(startRow, stopRow, dispStart, subpixStart,
665 rowWidth,
reinterpret_cast<unsigned short*
>(dst), srcStride, dstStride);
667 decodeSubpixelSSE2<false>(startRow, stopRow, dispStart, subpixStart,
668 rowWidth,
reinterpret_cast<unsigned short*
>(dst), srcStride, dstStride);
674 decodeSubpixelFallback(startRow, stopRow, dispStart, subpixStart, rowWidth,
675 reinterpret_cast<unsigned short*>(dst), srcStride, dstStride);
680 template <
bool alignedLoad>
681 void ImageProtocol::Pimpl::decodeSubpixelSSE2(
int startRow,
int stopRow,
const unsigned char* dispStart,
682 const unsigned char* subpixStart,
int width,
unsigned short* dst,
int srcStride,
int dstStride) {
683 if(width % 16 != 0) {
688 __m128i zero = _mm_set1_epi8(0x00);
689 __m128i subpixMask = _mm_set1_epi8(0x0f);
690 unsigned char* outPos = &
reinterpret_cast<unsigned char*
>(dst)[startRow*dstStride];
691 int outRowPadding = dstStride - 2*width;
693 for(
int y = startRow; y<stopRow; y++) {
694 const unsigned char* intPos = &dispStart[y*srcStride];
695 const unsigned char* intEndPos = &dispStart[y*srcStride + width];
696 const unsigned char* subpixPos = &subpixStart[y*srcStride];
698 for(; intPos < intEndPos;) {
700 __m128i subpixOffsets;
702 subpixOffsets = _mm_load_si128(reinterpret_cast<const __m128i*>(subpixPos));
704 subpixOffsets = _mm_loadu_si128(reinterpret_cast<const __m128i*>(subpixPos));
708 __m128i offsetsEven = _mm_and_si128(subpixOffsets, subpixMask);
709 __m128i offsetsUneven = _mm_and_si128(_mm_srli_epi16(subpixOffsets, 4), subpixMask);
711 for(
int i=0; i<2; i++) {
715 intDisps = _mm_load_si128(reinterpret_cast<const __m128i*>(intPos));
717 intDisps = _mm_loadu_si128(reinterpret_cast<const __m128i*>(intPos));
723 __m128i disps1 = _mm_slli_epi16(_mm_unpacklo_epi8(intDisps, zero), 4);
724 __m128i disps2 = _mm_slli_epi16(_mm_unpackhi_epi8(intDisps, zero), 4);
729 offsets = _mm_unpacklo_epi8(offsetsEven, offsetsUneven);
731 offsets = _mm_unpackhi_epi8(offsetsEven, offsetsUneven);
735 disps1 = _mm_or_si128(disps1, _mm_unpacklo_epi8(offsets, zero));
736 disps2 = _mm_or_si128(disps2, _mm_unpackhi_epi8(offsets, zero));
739 _mm_store_si128(reinterpret_cast<__m128i*>(outPos), disps1);
741 _mm_store_si128(reinterpret_cast<__m128i*>(outPos), disps2);
744 if(!alignedLoad && intPos >= intEndPos) {
751 outPos += outRowPadding;
757 template <
bool alignedLoad>
758 void ImageProtocol::Pimpl::decodeSubpixelAVX2(
int startRow,
int stopRow,
const unsigned char* dispStart,
759 const unsigned char* subpixStart,
int width,
unsigned short* dst,
int srcStride,
int dstStride) {
760 if(width % 32 != 0) {
767 __m256i zero = _mm256_set1_epi8(0x00);
768 __m256i subpixMask = _mm256_set1_epi8(0x0f);
769 unsigned char* outPos = &
reinterpret_cast<unsigned char*
>(dst)[startRow*dstStride];
770 int outRowPadding = dstStride - 2*width;
772 for(
int y = startRow; y<stopRow; y++) {
773 const unsigned char* intPos = &dispStart[y*srcStride];
774 const unsigned char* intEndPos = &dispStart[y*srcStride + width];
775 const unsigned char* subpixPos = &subpixStart[y*srcStride];
777 for(; intPos < intEndPos;) {
779 __m256i subpixOffsets;
782 subpixOffsets = _mm256_load_si256(reinterpret_cast<const __m256i*>(subpixPos));
784 subpixOffsets = _mm256_loadu_si256(reinterpret_cast<const __m256i*>(subpixPos));
788 __m256i offsetsEven = _mm256_and_si256(subpixOffsets, subpixMask);
789 __m256i offsetsUneven = _mm256_and_si256(_mm256_srli_epi16 (subpixOffsets, 4), subpixMask);
791 for(
int i=0; i<2; i++) {
795 intDisps = _mm256_load_si256(reinterpret_cast<const __m256i*>(intPos));
797 intDisps = _mm256_loadu_si256(reinterpret_cast<const __m256i*>(intPos));
802 __m256i intDispsMixup = _mm256_permute4x64_epi64(intDisps, 0xd8);
805 __m256i disps1 = _mm256_slli_epi16(_mm256_unpacklo_epi8(intDispsMixup, zero), 4);
806 __m256i disps2 = _mm256_slli_epi16(_mm256_unpackhi_epi8(intDispsMixup, zero), 4);
809 __m256i offsetsEvenMixup = _mm256_permute4x64_epi64(offsetsEven, 0xd8);
810 __m256i offsetsUnevenMixup = _mm256_permute4x64_epi64(offsetsUneven, 0xd8);
815 offsets = _mm256_unpacklo_epi8(offsetsEvenMixup, offsetsUnevenMixup);
817 offsets = _mm256_unpackhi_epi8(offsetsEvenMixup, offsetsUnevenMixup);
821 __m256i offsetsMixup = _mm256_permute4x64_epi64(offsets, 0xd8);
824 disps1 = _mm256_or_si256(disps1, _mm256_unpacklo_epi8(offsetsMixup, zero));
825 disps2 = _mm256_or_si256(disps2, _mm256_unpackhi_epi8(offsetsMixup, zero));
828 _mm256_store_si256(reinterpret_cast<__m256i*>(outPos), disps1);
830 _mm256_store_si256(reinterpret_cast<__m256i*>(outPos), disps2);
833 if(!alignedLoad && intPos >= intEndPos) {
840 outPos += outRowPadding;
845 void ImageProtocol::Pimpl::decodeSubpixelFallback(
int startRow,
int stopRow,
const unsigned char* dispStart,
846 const unsigned char* subpixStart,
int width,
unsigned short* dst,
int srcStride,
int dstStride) {
848 int dstStrideShort = dstStride/2;
851 for(
int y = startRow; y < stopRow; y++) {
852 for(
int x = 0; x < width; x++) {
854 unsigned short subpix = 0;
856 subpix = subpixStart[y*srcStride + x/2] & 0x0F;
858 subpix = subpixStart[y*srcStride + x/2] >> 4;
861 dst[y*dstStrideShort + x] = (
static_cast<unsigned short>(dispStart[y*srcStride + x]) << 4) | subpix;
void setTimestamp(int seconds, int microsec)
Sets the time at which this image pair has been captured.
void setHeight(int h)
Sets a new width for both images.
Exception class that is used for all protocol exceptions.
unsigned char * getNextReceiveBuffer(int &maxLength)
Returns the buffer for receiving the next network message.
void setSequenceNumber(unsigned int num)
Sets the sequence number for this image pair.
void setPixelFormat(int imageNumber, ImageFormat format)
Sets the pixel format for the given image.
bool getReceivedImagePair(ImagePair &imagePair)
Returns a received image when complete.
ImageProtocol(ProtocolType protType)
Creates a new instance for decoding / encoding network messages for the given network protocol...
void resetTransfer()
Aborts the transmission of the current transfer and performs a reset of the internal state...
unsigned char * getPixelData(int imageNumber) const
Returns the pixel data for the given image.
bool transferComplete()
Returns true if the current transfer has been completed.
A protocol for transmitting large blocks of data over a network.
const float * getQMatrix() const
Returns a pointer to the disparity-to-depth mapping matrix q.
int getWidth() const
Returns the width of each image.
void setDisparityRange(int minimum, int maximum)
Sets the value range for the disparity map contained in this image pair.
void setWidth(int w)
Sets a new width for both images.
void setQMatrix(const float *q)
Sets the pointer to the disparity-to-depth mapping matrix q.
void setRowStride(int imageNumber, int stride)
Sets a new row stride for the pixel data of one image.
void setTransferImagePair(const ImagePair &imagePair)
Sets a new image that will be transfer.
bool processReceivedMessage(int length)
Handles a received network message.
void setRawValidBytes(int validBytes)
Updates the number of valid bytes in a partial raw transfer.
void getTimestamp(int &seconds, int µsec) const
Returns the time at which this image pair has been captured.
void setPixelData(int imageNumber, unsigned char *pixelData)
Sets the pixel data for the given image.
void getDisparityRange(int &minimum, int &maximum) const
Gets the value range for the disparity map contained in this image pair. If the image pair does not c...
ProtocolType
Supported network protocols.
void resetReception()
Aborts the reception of the current image transfer and resets the internal state. ...
A set of two images, which are usually 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 setRawTransferData(const ImagePair &metaData, unsigned char *rawData, int firstTileWidth=0, int secondTileWidth=0, int validBytes=0x7FFFFFFF)
Sets the already pre-formatted image data for the next transfer.
int getHeight() const
Returns the height of each image.
unsigned int getSequenceNumber() const
Returns the sequence number for this image pair.
int getRowStride(int imageNumber) const
Returns the row stride for the pixel data of one image.
ImageFormat
Image formats that can be transferred.
bool imagesReceived() const
Returns true if the images of the current transfer have been received.
bool getPartiallyReceivedImagePair(ImagePair &imagePair, int &validRows, bool &complete)
Returns a partially received image.