libvisiontransfer  4.1.5
imageprotocol.cpp
1 /*******************************************************************************
2  * Copyright (c) 2017 Nerian Vision Technologies
3  *
4  * Permission is hereby granted, free of charge, to any person obtaining a copy
5  * of this software and associated documentation files (the "Software"), to deal
6  * in the Software without restriction, including without limitation the rights
7  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
8  * copies of the Software, and to permit persons to whom the Software is
9  * furnished to do so, subject to the following conditions:
10  *
11  * The above copyright notice and this permission notice shall be included in
12  * all copies or substantial portions of the Software.
13  *******************************************************************************/
14 
15 #include <cstring>
16 #include <iostream>
17 #include <limits>
18 #include <vector>
19 #include <memory>
20 #include <algorithm>
21 #include "visiontransfer/imageprotocol.h"
22 #include "visiontransfer/alignedallocator.h"
23 #include "visiontransfer/datablockprotocol.h"
24 #include "visiontransfer/exceptions.h"
25 
26 // Network headers
27 #ifdef _WIN32
28  #ifndef NOMINMAX
29  #define NOMINMAX
30  #endif
31  #include <winsock2.h>
32 #else
33  #include <arpa/inet.h>
34 #endif
35 
36 // SIMD Headers
37 #ifdef __AVX2__
38 #include <immintrin.h>
39 #elif __SSE2__
40 #include <emmintrin.h>
41 #endif
42 
43 using namespace std;
44 
45 /*************** Pimpl class containing all private members ***********/
46 
47 class ImageProtocol::Pimpl {
48 public:
49  Pimpl(ProtocolType protType);
50 
51  // Redeclaration of public members
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();
58  void resetTransfer();
59  bool getReceivedImagePair(ImagePair& imagePair);
60  bool getPartiallyReceivedImagePair(ImagePair& imagePair,
61  int& validRows, bool& complete);
62  bool imagesReceived() const;
63 
64  unsigned char* getNextReceiveBuffer(int& maxLength);
65 
66  bool processReceivedMessage(int length);
67  int getProspectiveMessageSize();
68  void resetReception();
69 
70 private:
71  // Header data transferred in the first packet
72 #pragma pack(push,1)
73  struct HeaderData{
74  unsigned char protocolVersion;
75  unsigned char padding0;
76 
77  unsigned short width;
78  unsigned short height;
79 
80  unsigned short firstTileWidth;
81  unsigned short secondTileWidth;
82 
83  unsigned char format0;
84  unsigned char format1;
85  unsigned char minDisparity;
86  unsigned char maxDisparity;
87 
88  unsigned int seqNum;
89  int timeSec;
90  int timeMicrosec;
91 
92  float q[16];
93 
94  unsigned char padding1[6]; // Pad to 32 bytes
95  };
96 #pragma pack(pop)
97 
98  static const unsigned char CURRENT_VERSION = 0x04;
99 
100  // Underlying protocol for data transfers
101  DataBlockProtocol dataProt;
102  ProtocolType protType;
103 
104  // Transfer related variables
105  bool headerTransferred;
106  std::vector<unsigned char> headerBuffer;
107  std::vector<unsigned char> rawBuffer;
108  unsigned char* rawData;
109  int rawValidBytes;
110  int rawDataLength;
111 
112  // Reception related variables
113  std::vector<unsigned char, AlignedAllocator<unsigned char> >decodeBuffer[2];
114  bool receiveHeaderParsed;
115  HeaderData receiveHeader;
116  int lastReceivedPayloadBytes[2];
117  int receiveTotalSize;
118  bool receptionDone;
119 
120  // Copies the transmission header to the given buffer
121  void copyHeaderToBuffer(const ImagePair& imagePair, int firstTileWidth,
122  int secondTileWidth, unsigned char* buffer);
123 
124  // Decodes header information from the received data
125  void tryDecodeHeader(const unsigned char* receivedData, int receivedBytes);
126 
127  // Decodes a received image from an interleaved buffer
128  unsigned char* decodeInterleaved(int imageNumber, int receivedBytes,
129  unsigned char* data, int& validRows, int& rowStride);
130 
131  // Decodes the 12-bit disparity map into 16-bit values
132  void decodeSubpixel(int startRow, int stopRow, unsigned const char* src,
133  unsigned char* dst, int srcStride, int dstStride, int rowWidth);
134 
135  // Various implementations of decodeSubpixel()
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);
139 
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);
143 
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);
146 
147  int getFrameSize(int width, int height, int firstTileWidth, int secondTileWidth,
148  ImagePair::ImageFormat format0, ImagePair::ImageFormat format1, int headerSize);
149 
150  int getFormatNibbles(ImagePair::ImageFormat format);
151 
152  void decodeTiledImage(int imageNumber, int lastReceivedPayloadBytes, int receivedPayloadBytes,
153  const unsigned char* data, int firstTileStride, int secondTileStride, int& validRows,
154  ImagePair::ImageFormat format);
155 
156  void decodeRowsFromTile(int startRow, int stopRow, unsigned const char* src,
157  unsigned char* dst, int srcStride, int dstStride, int tileWidth);
158 
159  void allocateDecodeBuffer(int imageNumber);
160 };
161 
162 
163 /******************** Stubs for all public members ********************/
164 
166  : pimpl(new Pimpl(protType)) {
167  // All initializations are done by the Pimpl class
168 }
169 
170 ImageProtocol::~ImageProtocol() {
171  delete pimpl;
172 }
173 
175  pimpl->setTransferImagePair(imagePair);
176 }
177 
179  unsigned char* imageData, int firstTileWidth, int secondTileWidth, int validBytes) {
180  pimpl->setRawTransferData(metaData, imageData, firstTileWidth, secondTileWidth, validBytes);
181 }
182 
183 void ImageProtocol::setRawValidBytes(int validBytes) {
184  pimpl->setRawValidBytes(validBytes);
185 }
186 
187 const unsigned char* ImageProtocol::getTransferMessage(int& length) {
188  return pimpl->getTransferMessage(length);
189 }
190 
192  return pimpl->transferComplete();
193 }
194 
196  pimpl->resetTransfer();
197 }
198 
200  return pimpl->getReceivedImagePair(imagePair);
201 }
202 
204  ImagePair& imagePair, int& validRows, bool& complete) {
205  return pimpl->getPartiallyReceivedImagePair(imagePair, validRows, complete);
206 }
207 
209  return pimpl->imagesReceived();
210 }
211 
212 unsigned char* ImageProtocol::getNextReceiveBuffer(int& maxLength) {
213  return pimpl->getNextReceiveBuffer(maxLength);
214 }
215 
217  return pimpl->processReceivedMessage(length);
218 }
219 
221  pimpl->resetReception();
222 }
223 
224 /******************** Implementation in pimpl class *******************/
225 
226 ImageProtocol::Pimpl::Pimpl(ProtocolType protType)
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));
234 
235  // Just after start-up we don't yet know the expected data size. So lets
236  // just allocate enough memory for one UDP packet
237  dataProt.setReceiveDataSize(DataBlockProtocol::MAX_UDP_BYTES_TRANSFER);
238 }
239 
240 void ImageProtocol::Pimpl::setTransferImagePair(const ImagePair& imagePair) {
241  if(imagePair.getPixelData(0) == nullptr || imagePair.getPixelData(1) == nullptr) {
242  throw ProtocolException("Image data is null pointer!");
243  }
244 
245  headerTransferred = false;
246 
247  // Set header as first piece of data
248  copyHeaderToBuffer(imagePair, 0, 0, &headerBuffer[0]);
249  dataProt.startTransfer();
250  dataProt.setTransferData(&headerBuffer[0], sizeof(HeaderData));
251 
252  // Make an interleaved copy of both images
253  int bytes0 = imagePair.getPixelFormat(0) == ImagePair::FORMAT_8_BIT ? 1 : 2;
254  int bytes1 = imagePair.getPixelFormat(1) == ImagePair::FORMAT_8_BIT ? 1 : 2;
255 
256  rawBuffer.resize(imagePair.getWidth()*imagePair.getHeight()*(bytes0 + bytes1) + sizeof(short));
257 
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++) {
262  memcpy(&rawBuffer[bufferOffset], &imagePair.getPixelData(0)[y*imagePair.getRowStride(0)], row0Size);
263  bufferOffset += row0Size;
264 
265  memcpy(&rawBuffer[bufferOffset], &imagePair.getPixelData(1)[y*imagePair.getRowStride(1)], row1Size);
266  bufferOffset += row1Size;
267  }
268 
269  rawData = &rawBuffer[0];
270  rawValidBytes = static_cast<int>(rawBuffer.size() - sizeof(short));
271 
272  rawDataLength = getFrameSize(imagePair.getWidth(), imagePair.getHeight(), 0, 0,
273  imagePair.getPixelFormat(0), imagePair.getPixelFormat(1), 0);
274 }
275 
276 void ImageProtocol::Pimpl::setRawTransferData(const ImagePair& metaData, unsigned char* rawData,
277  int firstTileWidth, int secondTileWidth, int validBytes) {
278  if(rawData == nullptr) {
279  throw ProtocolException("Image data is null pointer!");
280  } else if(metaData.getPixelFormat(0) != ImagePair::FORMAT_8_BIT) {
281  throw ProtocolException("First image must have 8-bit depth!");
282  }
283 
284  headerTransferred = false;
285 
286  // Set header as first piece of data
287  copyHeaderToBuffer(metaData, firstTileWidth, secondTileWidth, &headerBuffer[0]);
288  dataProt.startTransfer();
289  dataProt.setTransferData(&headerBuffer[0], sizeof(HeaderData));
290 
291  this->rawData = rawData;
292  rawValidBytes = validBytes;
293 
294  rawDataLength = getFrameSize(metaData.getWidth(), metaData.getHeight(),
295  firstTileWidth, secondTileWidth, metaData.getPixelFormat(0),
296  metaData.getPixelFormat(1), 0);
297 }
298 
299 void ImageProtocol::Pimpl::setRawValidBytes(int validBytes) {
300  rawValidBytes = validBytes;
301  if(headerTransferred) {
302  dataProt.setTransferValidBytes(validBytes);
303  }
304 }
305 
306 const unsigned char* ImageProtocol::Pimpl::getTransferMessage(int& length) {
307  const unsigned char* msg = dataProt.getTransferMessage(length);
308 
309  if(msg == nullptr && !headerTransferred && rawValidBytes > 0) {
310  // Transmitting the header is complete. Lets transfer the actual
311  // payload.
312  headerTransferred = true;
313  dataProt.setTransferData(rawData, rawDataLength, rawValidBytes);
314  msg = dataProt.getTransferMessage(length);
315  }
316 
317  return msg;
318 }
319 
320 bool ImageProtocol::Pimpl::transferComplete() {
321  return dataProt.transferComplete() && headerTransferred;
322 }
323 
324 int ImageProtocol::Pimpl::getFrameSize(int width, int height, int firstTileWidth,
325  int secondTileWidth, ImagePair::ImageFormat format0,
326  ImagePair::ImageFormat format1, int headerSize) {
327  int nibbles0 = format0 == ImagePair::FORMAT_8_BIT ? 2 : 3;
328  int nibbles1 = format1 == ImagePair::FORMAT_8_BIT ? 2 : 3;
329 
330  int effectiveWidth = firstTileWidth > 0 ? firstTileWidth + secondTileWidth : width;
331 
332  return (effectiveWidth * height * (nibbles0 + nibbles1)) /2 + headerSize;
333 }
334 
335 int ImageProtocol::Pimpl::getFormatNibbles(ImagePair::ImageFormat format) {
336  // A nibble is 4 bits
337  if(format == ImagePair::FORMAT_12_BIT) {
338  return 3;
339  } else {
340  return 2;
341  }
342 }
343 
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()));
356 
357  int minDisp = 0, maxDisp = 0;
358  imagePair.getDisparityRange(minDisp, maxDisp);
359  transferHeader->minDisparity = minDisp;
360  transferHeader->maxDisparity = maxDisp;
361 
362  int timeSec = 0, timeMicrosec = 0;
363  imagePair.getTimestamp(timeSec, timeMicrosec);
364  transferHeader->timeSec = static_cast<int>(htonl(static_cast<unsigned int>(timeSec)));
365  transferHeader->timeMicrosec = static_cast<int>(htonl(static_cast<unsigned int>(timeMicrosec)));
366 
367  if(imagePair.getQMatrix() != nullptr) {
368  memcpy(transferHeader->q, imagePair.getQMatrix(), sizeof(float)*16);
369  }
370 }
371 
372 void ImageProtocol::Pimpl::resetTransfer() {
373  dataProt.resetTransfer();
374 }
375 
376 unsigned char* ImageProtocol::Pimpl::getNextReceiveBuffer(int& maxLength) {
377  maxLength = dataProt.getMaxPayloadSize() + dataProt.getProtocolOverhead();
378  return dataProt.getNextReceiveBuffer(maxLength);
379 }
380 
381 bool ImageProtocol::Pimpl::processReceivedMessage(int length) {
382  receptionDone = false;
383 
384  // Add the received message
385  if(!dataProt.processReceivedMessage(length)) {
386  resetReception();
387  return false;
388  }
389 
390  int receivedBytes = 0;
391  const unsigned char* receivedData = dataProt.getReceivedData(receivedBytes);
392 
393  // Immediately try to decode the header
394  if(!receiveHeaderParsed && receivedBytes > 0) {
395  tryDecodeHeader(receivedData, receivedBytes);
396  }
397 
398  // Check if we have received a complete frame
399  if(receivedBytes == receiveTotalSize) {
400  receptionDone = true;
401  dataProt.finishReception();
402  } else if(receivedBytes > receiveTotalSize) {
403  // This is a corrupted frame
404  dataProt.resetReception();
405  return false;
406  }
407 
408  return true;
409 }
410 
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);
415 
416  if(receiveHeader.protocolVersion > CURRENT_VERSION ||
417  receiveHeader.protocolVersion < 4) {
418  throw ProtocolException("Protocol version mismatch!");
419  }
420 
421  // Convert byte order
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);
431 
432  // Make sure that the receive buffer is large enough
433  receiveTotalSize = getFrameSize(
434  receiveHeader.width,
435  receiveHeader.height,
436  receiveHeader.firstTileWidth,
437  receiveHeader.secondTileWidth,
438  static_cast<ImagePair::ImageFormat>(receiveHeader.format0),
439  static_cast<ImagePair::ImageFormat>(receiveHeader.format1),
440  sizeof(HeaderData));
441 
442  dataProt.setReceiveDataSize(receiveTotalSize);
443  receiveHeaderParsed = true;
444  }
445 }
446 
447 bool ImageProtocol::Pimpl::imagesReceived() const {
448  return receptionDone && receiveHeaderParsed;
449 }
450 
451 bool ImageProtocol::Pimpl::getReceivedImagePair(ImagePair& imagePair) {
452  bool complete = false;
453  int validRows;
454  bool ok = getPartiallyReceivedImagePair(imagePair, validRows, complete);
455 
456  return (ok && complete);
457 }
458 
459 bool ImageProtocol::Pimpl::getPartiallyReceivedImagePair(ImagePair& imagePair, int& validRows, bool& complete) {
460  imagePair.setWidth(0);
461  imagePair.setHeight(0);
462 
463  complete = false;
464 
465  if(!receiveHeaderParsed) {
466  // We haven't even received the image header yet
467  return false;
468  } else {
469  // We received at least some pixel data
470  int receivedBytes = 0;
471  unsigned char* data = dataProt.getReceivedData(receivedBytes);
472  if(receivedBytes == receiveTotalSize) {
473  // Receiving this frame has completed
474  dataProt.finishReception();
475  }
476 
477  validRows = 0;
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));
482 
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);
487 
488  imagePair.setRowStride(0, rowStride0);
489  imagePair.setRowStride(1, rowStride1);
490  imagePair.setPixelData(0, pixel0);
491  imagePair.setPixelData(1, pixel1);
492  imagePair.setQMatrix(receiveHeader.q);
493 
494  imagePair.setSequenceNumber(receiveHeader.seqNum);
495  imagePair.setTimestamp(receiveHeader.timeSec, receiveHeader.timeMicrosec);
496  imagePair.setDisparityRange(receiveHeader.minDisparity, receiveHeader.maxDisparity);
497 
498  validRows = min(validRows0, validRows1);
499 
500  if(validRows == receiveHeader.height) {
501  complete = true;
502  }
503 
504  if(receptionDone) {
505  // Reset everything for receiving the next image
506  resetReception();
507  }
508 
509  return true;
510  }
511 }
512 
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))) {
516  // We haven't yet received any data for the requested image
517  return nullptr;
518  }
519 
520  ImagePair::ImageFormat format = static_cast<ImagePair::ImageFormat>(
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));
524 
525  unsigned char* ret = nullptr;
526  int payloadBytes = receivedBytes - sizeof(HeaderData);
527 
528  if(receiveHeader.secondTileWidth == 0) {
529  int bufferOffset = sizeof(HeaderData) + imageNumber*receiveHeader.width * nibbles0/2;
530  int bufferRowStride = receiveHeader.width*(nibbles0 + nibbles1) / 2;
531 
532  if(format == ImagePair::FORMAT_12_BIT) {
533  // Perform 12-bit => 16 bit decoding
534  allocateDecodeBuffer(imageNumber);
535  validRows = payloadBytes / bufferRowStride;
536  rowStride = 2*receiveHeader.width;
537  int lastRow = lastReceivedPayloadBytes[imageNumber] / bufferRowStride;
538 
539  decodeSubpixel(lastRow, validRows, &data[bufferOffset],
540  &decodeBuffer[imageNumber][0], bufferRowStride, rowStride, receiveHeader.width);
541  ret = &decodeBuffer[imageNumber][0];
542  } else {
543  // No decoding is neccessary. We can just pass through the
544  // data pointer
545  ret = &data[bufferOffset];
546  rowStride = bufferRowStride;
547  validRows = payloadBytes / bufferRowStride;
548  }
549  } else {
550  // Decode the tiled transfer
551  decodeTiledImage(imageNumber,
552  lastReceivedPayloadBytes[imageNumber], payloadBytes,
553  data, receiveHeader.firstTileWidth * (nibbles0 + nibbles1) / 2,
554  receiveHeader.secondTileWidth * (nibbles0 + nibbles1) / 2,
555  validRows, format);
556  ret = &decodeBuffer[imageNumber][0];
557 
558  if(format == ImagePair::FORMAT_12_BIT) {
559  rowStride = 2*receiveHeader.width;
560  } else {
561  rowStride = receiveHeader.width;
562  }
563  }
564 
565  lastReceivedPayloadBytes[imageNumber] = payloadBytes;
566  return ret;
567 }
568 
569 void ImageProtocol::Pimpl::allocateDecodeBuffer(int imageNumber) {
570  ImagePair::ImageFormat format = static_cast<ImagePair::ImageFormat>(
571  imageNumber == 0 ? receiveHeader.format0 : receiveHeader.format1);
572  int bytesPerPixel = (format == ImagePair::FORMAT_12_BIT ? 2 : 1);
573  int bufferSize = receiveHeader.width * receiveHeader.height * bytesPerPixel;
574 
575  if(decodeBuffer[imageNumber].size() != static_cast<unsigned int>(bufferSize)) {
576  decodeBuffer[imageNumber].resize(bufferSize);
577  }
578 }
579 
580 void ImageProtocol::Pimpl::decodeTiledImage(int imageNumber, int lastReceivedPayloadBytes, int receivedPayloadBytes,
581  const unsigned char* data, int firstTileStride, int secondTileStride, int& validRows,
582  ImagePair::ImageFormat format) {
583 
584  // Allocate a decoding buffer
585  allocateDecodeBuffer(imageNumber);
586 
587  // Get beginning and end of first tile
588  int startFirstTile = lastReceivedPayloadBytes / firstTileStride;
589  int stopFirstTile = std::min(receivedPayloadBytes / firstTileStride,
590  static_cast<int>(receiveHeader.height));
591 
592  // Get beginning and end of second tile
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;
599 
600  // Decode first tile
601  if(format == ImagePair::FORMAT_12_BIT) {
602  decodeSubpixel(startFirstTile, stopFirstTile, &data[firstTileOffset], &decodeBuffer[imageNumber][0],
603  firstTileStride, 2*receiveHeader.width, receiveHeader.firstTileWidth);
604  } else {
605  decodeRowsFromTile(startFirstTile, stopFirstTile, &data[firstTileOffset],
606  &decodeBuffer[imageNumber][0], firstTileStride, receiveHeader.width,
607  receiveHeader.firstTileWidth);
608  }
609 
610  // Decode second tile
611  int secondTileOffset = sizeof(HeaderData) + receiveHeader.height*firstTileStride +
612  imageNumber * getFormatNibbles(static_cast<ImagePair::ImageFormat>(receiveHeader.format0)) * receiveHeader.secondTileWidth / 2;
613 
614  if(format == ImagePair::FORMAT_12_BIT) {
615  decodeSubpixel(startSecondTile, stopSecondTile,
616  &data[secondTileOffset], &decodeBuffer[imageNumber][2*receiveHeader.firstTileWidth],
617  secondTileStride, 2*receiveHeader.width, receiveHeader.secondTileWidth);
618  } else {
619  decodeRowsFromTile(startSecondTile, stopSecondTile, &data[secondTileOffset],
620  &decodeBuffer[imageNumber][receiveHeader.firstTileWidth],
621  secondTileStride, receiveHeader.width, receiveHeader.secondTileWidth);
622  }
623 
624  validRows = stopSecondTile;
625 }
626 
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);
631  }
632 }
633 
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;
641 }
642 
643 void ImageProtocol::Pimpl::decodeSubpixel(int startRow, int stopRow, unsigned const char* src,
644  unsigned char* dst, int srcStride, int dstStride, int rowWidth) {
645 
646  const unsigned char* dispStart = src;
647  const unsigned char* subpixStart = &src[rowWidth];
648 
649 # ifdef __AVX2__
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);
654  } else {
655  decodeSubpixelAVX2<false>(startRow, stopRow, dispStart, subpixStart,
656  rowWidth, reinterpret_cast<unsigned short*>(dst), srcStride, dstStride);
657  }
658  } else // We use the SSSE implementation as fall back if the image width is not
659  // dividable by 32
660 # endif
661 # ifdef __SSE2__
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);
666  } else {
667  decodeSubpixelSSE2<false>(startRow, stopRow, dispStart, subpixStart,
668  rowWidth, reinterpret_cast<unsigned short*>(dst), srcStride, dstStride);
669  }
670  } else // We use the SSSE implementation as fall back if the image width is not
671  // dividable by 32
672 # endif
673  {
674  decodeSubpixelFallback(startRow, stopRow, dispStart, subpixStart, rowWidth,
675  reinterpret_cast<unsigned short*>(dst), srcStride, dstStride);
676  }
677 }
678 
679 #ifdef __SSE2__
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) {
684  throw ProtocolException("Image width must be a multiple of 16!");
685  }
686 
687  // SSE optimized code
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;
692 
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];
697 
698  for(; intPos < intEndPos;) {
699  // Get subpix offsets
700  __m128i subpixOffsets;
701  if(alignedLoad) {
702  subpixOffsets = _mm_load_si128(reinterpret_cast<const __m128i*>(subpixPos));
703  } else {
704  subpixOffsets = _mm_loadu_si128(reinterpret_cast<const __m128i*>(subpixPos));
705  }
706  subpixPos += 16;
707 
708  __m128i offsetsEven = _mm_and_si128(subpixOffsets, subpixMask);
709  __m128i offsetsUneven = _mm_and_si128(_mm_srli_epi16(subpixOffsets, 4), subpixMask);
710 
711  for(int i=0; i<2; i++) {
712  // Load integer disparities
713  __m128i intDisps;
714  if(alignedLoad) {
715  intDisps = _mm_load_si128(reinterpret_cast<const __m128i*>(intPos));
716  } else {
717  intDisps = _mm_loadu_si128(reinterpret_cast<const __m128i*>(intPos));
718  }
719 
720  intPos += 16;
721 
722  // Get integer disparities shifted by 4
723  __m128i disps1 = _mm_slli_epi16(_mm_unpacklo_epi8(intDisps, zero), 4);
724  __m128i disps2 = _mm_slli_epi16(_mm_unpackhi_epi8(intDisps, zero), 4);
725 
726  // Unpack subpixel offsets for selected disparities
727  __m128i offsets;
728  if(i == 0) {
729  offsets = _mm_unpacklo_epi8(offsetsEven, offsetsUneven);
730  } else {
731  offsets = _mm_unpackhi_epi8(offsetsEven, offsetsUneven);
732  }
733 
734  // Add subpixel offsets to integer disparities
735  disps1 = _mm_or_si128(disps1, _mm_unpacklo_epi8(offsets, zero));
736  disps2 = _mm_or_si128(disps2, _mm_unpackhi_epi8(offsets, zero));
737 
738  // Store result
739  _mm_store_si128(reinterpret_cast<__m128i*>(outPos), disps1);
740  outPos += 16;
741  _mm_store_si128(reinterpret_cast<__m128i*>(outPos), disps2);
742  outPos += 16;
743 
744  if(!alignedLoad && intPos >= intEndPos) {
745  // In the non-aligned case we might need one iteration less
746  break;
747  }
748  }
749  }
750 
751  outPos += outRowPadding;
752  }
753 }
754 #endif
755 
756 # ifdef __AVX2__
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) {
761  // We use the SSE implementation as fall back if the image size isn't
762  // a multiple of
763  throw ProtocolException("Image width must be a multiple of 32!");
764  }
765 
766  // AVX2 optimized code
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;
771 
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];
776 
777  for(; intPos < intEndPos;) {
778  // Get subpix offsets
779  __m256i subpixOffsets;
780 
781  if(alignedLoad) {
782  subpixOffsets = _mm256_load_si256(reinterpret_cast<const __m256i*>(subpixPos));
783  } else {
784  subpixOffsets = _mm256_loadu_si256(reinterpret_cast<const __m256i*>(subpixPos));
785  }
786  subpixPos += 32;
787 
788  __m256i offsetsEven = _mm256_and_si256(subpixOffsets, subpixMask);
789  __m256i offsetsUneven = _mm256_and_si256(_mm256_srli_epi16 (subpixOffsets, 4), subpixMask);
790 
791  for(int i=0; i<2; i++) {
792  // Load integer disparities
793  __m256i intDisps;
794  if(alignedLoad) {
795  intDisps = _mm256_load_si256(reinterpret_cast<const __m256i*>(intPos));
796  } else {
797  intDisps = _mm256_loadu_si256(reinterpret_cast<const __m256i*>(intPos));
798  }
799  intPos += 32;
800 
801  // Stupid AVX2 unpack mixes everything up! Lets swap the register beforehand.
802  __m256i intDispsMixup = _mm256_permute4x64_epi64(intDisps, 0xd8);
803 
804  // Get integer disparities shifted by 4
805  __m256i disps1 = _mm256_slli_epi16(_mm256_unpacklo_epi8(intDispsMixup, zero), 4);
806  __m256i disps2 = _mm256_slli_epi16(_mm256_unpackhi_epi8(intDispsMixup, zero), 4);
807 
808  // Unpack swap again :-(
809  __m256i offsetsEvenMixup = _mm256_permute4x64_epi64(offsetsEven, 0xd8);
810  __m256i offsetsUnevenMixup = _mm256_permute4x64_epi64(offsetsUneven, 0xd8);
811 
812  // Unpack subpixel offsets for selected disparities
813  __m256i offsets;
814  if(i == 0) {
815  offsets = _mm256_unpacklo_epi8(offsetsEvenMixup, offsetsUnevenMixup);
816  } else {
817  offsets = _mm256_unpackhi_epi8(offsetsEvenMixup, offsetsUnevenMixup);
818  }
819 
820  // And again!!
821  __m256i offsetsMixup = _mm256_permute4x64_epi64(offsets, 0xd8);
822 
823  // Add subpixel offsets to integer disparities
824  disps1 = _mm256_or_si256(disps1, _mm256_unpacklo_epi8(offsetsMixup, zero));
825  disps2 = _mm256_or_si256(disps2, _mm256_unpackhi_epi8(offsetsMixup, zero));
826 
827  // Store result
828  _mm256_store_si256(reinterpret_cast<__m256i*>(outPos), disps1);
829  outPos += 32;
830  _mm256_store_si256(reinterpret_cast<__m256i*>(outPos), disps2);
831  outPos += 32;
832 
833  if(!alignedLoad && intPos >= intEndPos) {
834  // In the non-aligned case we might need one iteration less
835  break;
836  }
837  }
838  }
839 
840  outPos += outRowPadding;
841  }
842 }
843 # endif
844 
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) {
847 
848  int dstStrideShort = dstStride/2;
849 
850  // Non-SSE version
851  for(int y = startRow; y < stopRow; y++) {
852  for(int x = 0; x < width; x++) {
853 
854  unsigned short subpix = 0;
855  if(x % 2 == 0) {
856  subpix = subpixStart[y*srcStride + x/2] & 0x0F;
857  } else {
858  subpix = subpixStart[y*srcStride + x/2] >> 4;
859  }
860 
861  dst[y*dstStrideShort + x] = (static_cast<unsigned short>(dispStart[y*srcStride + x]) << 4) | subpix;
862  }
863  }
864 }
void setTimestamp(int seconds, int microsec)
Sets the time at which this image pair has been captured.
Definition: imagepair.h:127
void setHeight(int h)
Sets a new width for both images.
Definition: imagepair.h:65
Exception class that is used for all protocol exceptions.
Definition: exceptions.h:23
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.
Definition: imagepair.h:116
void setPixelFormat(int imageNumber, ImageFormat format)
Sets the pixel format for the given image.
Definition: imagepair.h:86
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.
Definition: imagepair.h:182
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.
Definition: imagepair.h:190
int getWidth() const
Returns the width of each image.
Definition: imagepair.h:147
void setDisparityRange(int minimum, int maximum)
Sets the value range for the disparity map contained in this image pair.
Definition: imagepair.h:139
void setWidth(int w)
Sets a new width for both images.
Definition: imagepair.h:60
void setQMatrix(const float *q)
Sets the pointer to the disparity-to-depth mapping matrix q.
Definition: imagepair.h:109
void setRowStride(int imageNumber, int stride)
Sets a new row stride for the pixel data of one image.
Definition: imagepair.h:74
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 &microsec) const
Returns the time at which this image pair has been captured.
Definition: imagepair.h:206
void setPixelData(int imageNumber, unsigned char *pixelData)
Sets the pixel data for the given image.
Definition: imagepair.h:98
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...
Definition: imagepair.h:219
ProtocolType
Supported network protocols.
Definition: imageprotocol.h:39
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.
Definition: imagepair.h:30
ImageFormat getPixelFormat(int imageNumber) const
Returns the pixel format for the given image.
Definition: imagepair.h:171
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.
8-bit greyscale format
Definition: imagepair.h:37
int getHeight() const
Returns the height of each image.
Definition: imagepair.h:152
unsigned int getSequenceNumber() const
Returns the sequence number for this image pair.
Definition: imagepair.h:197
int getRowStride(int imageNumber) const
Returns the row stride for the pixel data of one image.
Definition: imagepair.h:160
ImageFormat
Image formats that can be transferred.
Definition: imagepair.h:35
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.
Nerian Vision Technologies