libvisiontransfer  8.1.0
imageprotocol.cpp
1 /*******************************************************************************
2  * Copyright (c) 2020 Nerian Vision GmbH
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 #include "visiontransfer/bitconversions.h"
26 #include "visiontransfer/internalinformation.h"
27 
28 // Network headers
29 #ifdef _WIN32
30  #ifndef NOMINMAX
31  #define NOMINMAX
32  #endif
33  #include <winsock2.h>
34 #else
35  #include <arpa/inet.h>
36 #endif
37 
38 //#define LOG_WARN(expr)
39 #define LOG_WARN(expr) std::cerr << "DataBlockProtocol: " << expr << std::endl
40 
41 using namespace std;
42 using namespace visiontransfer;
43 using namespace visiontransfer::internal;
44 
45 namespace visiontransfer {
46 
47 /*************** Pimpl class containing all private members ***********/
48 
49 class ImageProtocol::Pimpl {
50 public:
51 
52  static const int IMAGE_HEADER_OFFSET = sizeof(DataBlockProtocol::HeaderPreamble) + 10;
53 
54  Pimpl(bool server, ProtocolType protType, int maxUdpPacketSize);
55 
56  // Redeclaration of public members
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();
63  void resetTransfer();
64  bool getReceivedImageSet(ImageSet& imageSet);
65  bool getPartiallyReceivedImageSet(ImageSet& imageSet,
66  int& validRows, bool& complete);
67  bool imagesReceived() const;
68 
69  unsigned char* getNextReceiveBuffer(int& maxLength);
70 
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();
78 
79  std::string statusReport();
80 
81 private:
82  unsigned short MAGIC_SEQUECE = 0x3D15;
83 
84  // Header data transferred in the first packet
85 #pragma pack(push,1)
86  struct HeaderDataLegacy {
87  unsigned short magic;
88 
89  unsigned char protocolVersion;
90  unsigned char isRawImagePair_OBSOLETE;
91 
92  unsigned short width;
93  unsigned short height;
94 
95  unsigned short firstTileWidth;
96  unsigned short lastTileWidth;
97 
98  unsigned char format0;
99  unsigned char format1;
100  unsigned short minDisparity;
101  unsigned short maxDisparity;
102  unsigned char subpixelFactor;
103 
104  unsigned int seqNum;
105  int timeSec;
106  int timeMicrosec;
107 
108  float q[16];
109 
110  unsigned short middleTilesWidth;
111  };
112  // Header data v2: extensible and forwards-compatible
113  struct HeaderDataV2: public HeaderDataLegacy {
114  unsigned short totalHeaderSize;
115  unsigned short flags;
116  unsigned char numberOfImages;
117  unsigned char format2;
118  enum FlagBits {
119  NEW_STYLE_TRANSFER = 1,
120  HEADER_V3 = 2,
121  // future protocol extensions should mark a new bit here
122  };
123  };
124  // Header data v3, adds arbitrary image channel assignments
125  struct HeaderData: public HeaderDataV2 {
126  // HEADER_V3 bit implies that this extension is present,
127  // declaring arbitrary channel roles for each of numberOfImages active channels.
128  // If not present, is it an old sender that always sends two images
129  // (channel 0: left, channel 1: right or disparity (if active))
130  unsigned char imageTypes[8];
131  };
132 #pragma pack(pop)
133 
134  // Underlying protocol for data transfers
135  DataBlockProtocol dataProt;
136  ProtocolType protType;
137 
138  // Transfer related variables
139  std::vector<unsigned char> headerBuffer;
140 
141  // Reception related variables
142  std::vector<unsigned char, AlignedAllocator<unsigned char> >decodeBuffer[ImageSet::MAX_SUPPORTED_IMAGES];
143  bool receiveHeaderParsed;
144  HeaderData receiveHeader;
145  int lastReceivedPayloadBytes[ImageSet::MAX_SUPPORTED_IMAGES];
146  bool receptionDone;
147 
148  // Copies the transmission header to the given buffer
149  void copyHeaderToBuffer(const ImageSet& imageSet, int firstTileWidth,
150  int middleTilesWidth, int lastTileWidth, unsigned char* buffer);
151 
152  // Decodes header information from the received data
153  void tryDecodeHeader(const unsigned char* receivedData, int receivedBytes);
154 
155  // Decodes a received image from a non-interleaved buffer
156  unsigned char* decodeNoninterleaved(int imageNumber, int numImages, int receivedBytes,
157  unsigned char* data, int& validRows, int& rowStride);
158 
159  // Decodes a received image from an interleaved buffer
160  unsigned char* decodeInterleaved(int imageNumber, int numImages, int receivedBytes,
161  unsigned char* data, int& validRows, int& rowStride);
162 
163  int getNumTiles(int width, int firstTileWidth, int middleTilesWidth, int lastTileWidth);
164 
165  int getFrameSize(int width, int height, int firstTileWidth, int middleTilesWidth,
166  int lastTileWidth, int totalBits);
167 
168  int getFormatBits(ImageSet::ImageFormat format, bool afterDecode);
169 
170  void decodeTiledImage(int imageNumber, int lastReceivedPayloadBytes, int receivedPayloadBytes,
171  const unsigned char* data, int firstTileStride, int middleTilesStride, int lastTileStride,
172  int& validRows, ImageSet::ImageFormat format, bool dataIsInterleaved);
173 
174  void decodeRowsFromTile(int startRow, int stopRow, unsigned const char* src,
175  unsigned char* dst, int srcStride, int dstStride, int tileWidth);
176 
177  void allocateDecodeBuffer(int imageNumber);
178 };
179 
180 
181 /******************** Stubs for all public members ********************/
182 
183 ImageProtocol::ImageProtocol(bool server, ProtocolType protType, int maxUdpPacketSize)
184  : pimpl(new Pimpl(server, protType, maxUdpPacketSize)) {
185  // All initializations are done by the Pimpl class
186 }
187 
188 ImageProtocol::~ImageProtocol() {
189  delete pimpl;
190 }
191 
193  pimpl->setTransferImageSet(imageSet);
194 }
195 
196 void ImageProtocol::setRawTransferData(const ImageSet& metaData, const std::vector<unsigned char*>& imageData,
197  int firstTileWidth, int middleTilesWidth, int lastTileWidth, int validBytes) {
198  pimpl->setRawTransferData(metaData, imageData, firstTileWidth, middleTilesWidth, lastTileWidth, validBytes);
199 }
200 
201 void ImageProtocol::setRawValidBytes(const std::vector<int>& validBytesVec) {
202  pimpl->setRawValidBytes(validBytesVec);
203 }
204 
205 const unsigned char* ImageProtocol::getTransferMessage(int& length) {
206  return pimpl->getTransferMessage(length);
207 }
208 
210  return pimpl->transferComplete();
211 }
212 
214  pimpl->resetTransfer();
215 }
216 
218  return pimpl->getReceivedImageSet(imageSet);
219 }
220 
222  ImageSet& imageSet, int& validRows, bool& complete) {
223  return pimpl->getPartiallyReceivedImageSet(imageSet, validRows, complete);
224 }
225 
227  return pimpl->imagesReceived();
228 }
229 
230 unsigned char* ImageProtocol::getNextReceiveBuffer(int& maxLength) {
231  return pimpl->getNextReceiveBuffer(maxLength);
232 }
233 
235  pimpl->processReceivedMessage(length);
236 }
237 
239  return pimpl->getNumDroppedFrames();
240 }
241 
243  pimpl->resetReception();
244 }
245 
247  return pimpl->isConnected();
248 }
249 
250 const unsigned char* ImageProtocol::getNextControlMessage(int& length) {
251  return pimpl->getNextControlMessage(length);
252 }
253 
255  return pimpl->newClientConnected();
256 }
257 
258 /******************** Implementation in pimpl class *******************/
259 
260 ImageProtocol::Pimpl::Pimpl(bool server, ProtocolType protType, int maxUdpPacketSize)
261  :dataProt(server, (DataBlockProtocol::ProtocolType)protType,
262  maxUdpPacketSize), protType(protType),
263  receiveHeaderParsed(false), lastReceivedPayloadBytes{0},
264  receptionDone(false) {
265  headerBuffer.resize(sizeof(HeaderData) + 128);
266  memset(&headerBuffer[0], 0, sizeof(headerBuffer.size()));
267  memset(&receiveHeader, 0, sizeof(receiveHeader));
268 }
269 
270 void ImageProtocol::Pimpl::setTransferImageSet(const ImageSet& imageSet) {
271  for (int i=0; i<imageSet.getNumberOfImages(); ++i) {
272  if(imageSet.getPixelData(i) == nullptr) {
273  throw ProtocolException("Image data is null pointer!");
274  }
275  }
276 
277  // Set header as first piece of data
278  copyHeaderToBuffer(imageSet, 0, 0, 0, &headerBuffer[IMAGE_HEADER_OFFSET]);
279  dataProt.resetTransfer();
280  int numTransferBlocks = imageSet.getNumberOfImages();
281  dataProt.setTransferHeader(&headerBuffer[IMAGE_HEADER_OFFSET], sizeof(HeaderData), numTransferBlocks);
282  for (int i=0; i<imageSet.getNumberOfImages(); ++i) {
283  int bits = getFormatBits(imageSet.getPixelFormat(i), false);
284  int rawDataLength = getFrameSize(imageSet.getWidth(), imageSet.getHeight(), 0, 0, 0, bits);
285  dataProt.setTransferBytes(i, rawDataLength);
286  }
287 
288  // Perform 12 bit packed encoding if necessary
289  int bits[ImageSet::MAX_SUPPORTED_IMAGES] = {0};
290  int rowSize[ImageSet::MAX_SUPPORTED_IMAGES] = {0};
291  const unsigned char* pixelData[ImageSet::MAX_SUPPORTED_IMAGES] = {nullptr};
292  std::vector<unsigned char> encodingBuffer[ImageSet::MAX_SUPPORTED_IMAGES];
293 
294  for(int i = 0; i<imageSet.getNumberOfImages(); i++) {
295  bits[i] = getFormatBits(imageSet.getPixelFormat(i), false);
296  rowSize[i] = imageSet.getWidth()*bits[i]/8;
297 
298  if(imageSet.getPixelFormat(i) != ImageSet::FORMAT_12_BIT_MONO) {
299  pixelData[i] = imageSet.getPixelData(i);
300  } else {
301  encodingBuffer[i].resize(rowSize[i] * imageSet.getHeight());
302  BitConversions::encode12BitPacked(0, imageSet.getHeight(), imageSet.getPixelData(i),
303  &encodingBuffer[i][0], imageSet.getRowStride(i), rowSize[i], imageSet.getWidth());
304  pixelData[i] = &encodingBuffer[i][0];
305  }
306  }
307 
308  for (int i=0; i<imageSet.getNumberOfImages(); ++i) {
309  dataProt.setTransferData(i, const_cast<unsigned char*>(pixelData[i])); // these are always reserved memory or untile buffers
310  }
311 }
312 
313 void ImageProtocol::Pimpl::setRawTransferData(const ImageSet& metaData, const std::vector<unsigned char*>& rawData,
314  int firstTileWidth, int middleTilesWidth, int lastTileWidth, int validBytes) {
315  if(static_cast<int>(rawData.size()) != metaData.getNumberOfImages()) {
316  throw ProtocolException("Mismatch between metadata and number of image buffers!");
317  }
318 
319  // Set header as first piece of data
320  copyHeaderToBuffer(metaData, firstTileWidth, middleTilesWidth, lastTileWidth, &headerBuffer[IMAGE_HEADER_OFFSET]);
321  dataProt.resetTransfer();
322  int numTransferBlocks = metaData.getNumberOfImages();
323  dataProt.setTransferHeader(&headerBuffer[IMAGE_HEADER_OFFSET], sizeof(HeaderData), numTransferBlocks);
324  // Now set the size per channel (replaces old final size argument to setTransferHeader()
325  for (int i=0; i<metaData.getNumberOfImages(); ++i) {
326  int rawDataLength = getFrameSize(metaData.getWidth(), metaData.getHeight(),
327  firstTileWidth, middleTilesWidth, lastTileWidth, metaData.getBitsPerPixel(i));
328  dataProt.setTransferBytes(i, rawDataLength);
329  }
330 
331  for (int i=0; i<metaData.getNumberOfImages(); ++i) {
332  dataProt.setTransferData(i, rawData[i]);
333  }
334 }
335 
336 void ImageProtocol::Pimpl::setRawValidBytes(const std::vector<int>& validBytesVec) {
337  for (int i=0; i<static_cast<int>(validBytesVec.size()); ++i) {
338  dataProt.setTransferValidBytes(i, validBytesVec[i]);
339  }
340 }
341 
342 const unsigned char* ImageProtocol::Pimpl::getTransferMessage(int& length) {
343  const unsigned char* msg = dataProt.getTransferMessage(length);
344 
345  if(msg == nullptr) {
346  msg = dataProt.getTransferMessage(length);
347  }
348 
349  return msg;
350 }
351 
352 bool ImageProtocol::Pimpl::transferComplete() {
353  return dataProt.transferComplete();
354 }
355 
356 int ImageProtocol::Pimpl::getNumTiles(int width, int firstTileWidth, int middleTilesWidth, int lastTileWidth) {
357  if(lastTileWidth == 0) {
358  return 1;
359  } else if(middleTilesWidth == 0) {
360  return 2;
361  } else {
362  int tileWidth = firstTileWidth + lastTileWidth - middleTilesWidth;
363  return (width - 2*tileWidth + firstTileWidth + lastTileWidth) / (firstTileWidth + lastTileWidth - tileWidth);
364  }
365 }
366 
367 int ImageProtocol::Pimpl::getFrameSize(int width, int height, int firstTileWidth,
368  int middleTilesWidth, int lastTileWidth, int totalBits) {
369  return (width * height * totalBits) /8;
370 }
371 
372 int ImageProtocol::Pimpl::getFormatBits(ImageSet::ImageFormat format, bool afterDecode) {
373  if(afterDecode) {
374  return ImageSet::getBytesPerPixel(format)*8;
375  } else {
376  switch(format) {
377  case ImageSet::FORMAT_8_BIT_MONO: return 8;
378  case ImageSet::FORMAT_12_BIT_MONO: return 12;
379  case ImageSet::FORMAT_8_BIT_RGB: return 24;
380  default: throw ProtocolException("Illegal pixel format!");
381  }
382  }
383 }
384 
385 void ImageProtocol::Pimpl::copyHeaderToBuffer(const ImageSet& imageSet,
386  int firstTileWidth, int middleTilesWidth, int lastTileWidth, unsigned char* buffer) {
387  HeaderData* transferHeader = reinterpret_cast<HeaderData*>(buffer);
388  memset(transferHeader, 0, sizeof(*transferHeader));
389  transferHeader->magic = htons(MAGIC_SEQUECE);
390  transferHeader->protocolVersion = InternalInformation::CURRENT_PROTOCOL_VERSION;
391  transferHeader->isRawImagePair_OBSOLETE = 0;
392  transferHeader->width = htons(imageSet.getWidth());
393  transferHeader->height = htons(imageSet.getHeight());
394  transferHeader->firstTileWidth = htons(firstTileWidth);
395  transferHeader->lastTileWidth = htons(lastTileWidth);
396  transferHeader->middleTilesWidth = htons(middleTilesWidth);
397  transferHeader->format0 = static_cast<unsigned char>(imageSet.getPixelFormat(0));
398  transferHeader->format1 = (imageSet.getNumberOfImages() <= 1) ? 0 : static_cast<unsigned char>(imageSet.getPixelFormat(1));
399  transferHeader->seqNum = static_cast<unsigned int>(htonl(imageSet.getSequenceNumber()));
400  transferHeader->format2 = (imageSet.getNumberOfImages() <= 2) ? 0 : static_cast<unsigned char>(imageSet.getPixelFormat(2));
401  transferHeader->numberOfImages = static_cast<unsigned char>(imageSet.getNumberOfImages());
402  transferHeader->totalHeaderSize = htons(sizeof(HeaderData));
403  transferHeader->flags = htons(HeaderData::FlagBits::NEW_STYLE_TRANSFER | HeaderData::FlagBits::HEADER_V3);
404 
405  int minDisp = 0, maxDisp = 0;
406  imageSet.getDisparityRange(minDisp, maxDisp);
407  transferHeader->minDisparity = minDisp;
408  transferHeader->maxDisparity = maxDisp;
409 
410  transferHeader->subpixelFactor = imageSet.getSubpixelFactor();
411 
412  int timeSec = 0, timeMicrosec = 0;
413  imageSet.getTimestamp(timeSec, timeMicrosec);
414  transferHeader->timeSec = static_cast<int>(htonl(static_cast<unsigned int>(timeSec)));
415  transferHeader->timeMicrosec = static_cast<int>(htonl(static_cast<unsigned int>(timeMicrosec)));
416 
417  int numImageChannels = 0;
418  for (int i=0; i<(int) sizeof(transferHeader->imageTypes); ++i) {
419  transferHeader->imageTypes[i] = static_cast<unsigned char>(ImageSet::ImageType::IMAGE_UNDEFINED);
420  }
421  int idx = imageSet.getIndexOf(ImageSet::ImageType::IMAGE_LEFT);
422  if (idx>=0) {
423  transferHeader->imageTypes[idx] = static_cast<unsigned char>(ImageSet::ImageType::IMAGE_LEFT);
424  numImageChannels++;
425  }
426  idx = imageSet.getIndexOf(ImageSet::ImageType::IMAGE_RIGHT);
427  if (idx>=0) {
428  transferHeader->imageTypes[idx] = static_cast<unsigned char>(ImageSet::ImageType::IMAGE_RIGHT);
429  numImageChannels++;
430  }
431  idx = imageSet.getIndexOf(ImageSet::ImageType::IMAGE_DISPARITY);
432  if (idx>=0) {
433  transferHeader->imageTypes[idx] = static_cast<unsigned char>(ImageSet::ImageType::IMAGE_DISPARITY);
434  numImageChannels++;
435  }
436  if (numImageChannels != imageSet.getNumberOfImages()) {
437  throw std::runtime_error("Mismatch between reported number of images and enabled channel selection!");
438  }
439 
440 
441  if(imageSet.getQMatrix() != nullptr) {
442  memcpy(transferHeader->q, imageSet.getQMatrix(), sizeof(float)*16);
443  }
444 }
445 
446 void ImageProtocol::Pimpl::resetTransfer() {
447  dataProt.resetTransfer();
448 }
449 
450 unsigned char* ImageProtocol::Pimpl::getNextReceiveBuffer(int& maxLength) {
451  maxLength = dataProt.getMaxReceptionSize();
452  return dataProt.getNextReceiveBuffer(maxLength);
453 }
454 
455 void ImageProtocol::Pimpl::processReceivedMessage(int length) {
456  receptionDone = false;
457 
458  // Add the received message
459  dataProt.processReceivedMessage(length, receptionDone);
460 
461  int receivedBytes = 0;
462  dataProt.getReceivedData(receivedBytes);
463 
464  // Immediately try to decode the header
465  if(!receiveHeaderParsed) {
466  int headerLen = 0;
467  unsigned char* headerData = dataProt.getReceivedHeader(headerLen);
468  if(headerData != nullptr) {
469  tryDecodeHeader(headerData, headerLen);
470  }
471  }
472 }
473 
474 void ImageProtocol::Pimpl::tryDecodeHeader(const
475 unsigned char* receivedData, int receivedBytes) {
476  // Extra data fields that have been added to the header. Must be
477  // removed when the protocol version number is updated
478  constexpr int optionalDataSize = sizeof(receiveHeader.middleTilesWidth);
479  constexpr int mandatoryDataSize = static_cast<int>(sizeof(HeaderDataLegacy)) - optionalDataSize;
480  constexpr int fullyExtensibleHeaderSize = static_cast<int>(sizeof(HeaderDataV2));
481  bool isCompleteHeader = false;
482 
483  if(receivedBytes >= mandatoryDataSize) {
484  if (receivedBytes < fullyExtensibleHeaderSize) {
485  *(static_cast<HeaderDataLegacy*>(&receiveHeader)) = *reinterpret_cast<const HeaderDataLegacy*>(receivedData);
486  } else {
487  receiveHeader = *reinterpret_cast<const HeaderData*>(receivedData);
488  isCompleteHeader = true;
489  }
490  if(receiveHeader.magic != htons(MAGIC_SEQUECE)) {
491  // Let's not call this an error. Perhaps it's just not a header
492  // packet
493  return;
494  }
495 
496  if(receiveHeader.protocolVersion != InternalInformation::CURRENT_PROTOCOL_VERSION) {
497  throw ProtocolException("Protocol version mismatch!");
498  }
499 
500  // Convert byte order
501  receiveHeader.width = ntohs(receiveHeader.width);
502  receiveHeader.height = ntohs(receiveHeader.height);
503  receiveHeader.firstTileWidth = ntohs(receiveHeader.firstTileWidth);
504  receiveHeader.lastTileWidth = ntohs(receiveHeader.lastTileWidth);
505 
506  receiveHeader.timeSec = static_cast<int>(
507  ntohl(static_cast<unsigned int>(receiveHeader.timeSec)));
508  receiveHeader.timeMicrosec = static_cast<int>(
509  ntohl(static_cast<unsigned int>(receiveHeader.timeMicrosec)));
510  receiveHeader.seqNum = ntohl(receiveHeader.seqNum);
511 
512  // Optional data items
513  if(receivedBytes >= mandatoryDataSize + optionalDataSize) {
514  receiveHeader.middleTilesWidth = ntohs(receiveHeader.middleTilesWidth);
515  } else {
516  receiveHeader.middleTilesWidth = 0;
517  }
518  if (isCompleteHeader) {
519  // This is a header of v2 or above, which self-reports its extension level in the flags field
520  receiveHeader.totalHeaderSize = ntohs(receiveHeader.totalHeaderSize);
521  receiveHeader.flags = ntohs(receiveHeader.flags);
522  } else {
523  // Infer missing fields for legacy compatibility transfers
524  receiveHeader.totalHeaderSize = (receivedBytes <= mandatoryDataSize) ? mandatoryDataSize : static_cast<int>(sizeof(HeaderDataLegacy));
525  receiveHeader.flags = 0;
526  receiveHeader.numberOfImages = 2;
527  receiveHeader.format2 = 0;
528  }
529 
530  receiveHeaderParsed = true;
531  }
532 }
533 
534 bool ImageProtocol::Pimpl::imagesReceived() const {
535  return receptionDone && receiveHeaderParsed;
536 }
537 
538 bool ImageProtocol::Pimpl::getReceivedImageSet(ImageSet& imageSet) {
539  bool complete = false;
540  int validRows;
541  bool ok = getPartiallyReceivedImageSet(imageSet, validRows, complete);
542 
543  return (ok && complete);
544 }
545 
546 bool ImageProtocol::Pimpl::getPartiallyReceivedImageSet(ImageSet& imageSet, int& validRows, bool& complete) {
547  imageSet.setWidth(0);
548  imageSet.setHeight(0);
549 
550  complete = false;
551 
552  if(!receiveHeaderParsed) {
553  // We haven't even received the image header yet
554  return false;
555  } else {
556  // We received at least some pixel data
557  imageSet.setNumberOfImages(receiveHeader.numberOfImages);
558  bool flaggedDisparityPair = (receiveHeader.isRawImagePair_OBSOLETE == 0); // only meaningful in headers <=V2
559  bool isInterleaved = (receiveHeader.flags & HeaderData::FlagBits::NEW_STYLE_TRANSFER) == 0;
560  bool arbitraryChannels = (receiveHeader.flags & HeaderData::FlagBits::HEADER_V3) > 0;
561 
562  // Forward compatibility check: mask out all known flag bits and see what remains
563  unsigned short unaccountedFlags = receiveHeader.flags & ~(HeaderData::FlagBits::NEW_STYLE_TRANSFER | HeaderData::FlagBits::HEADER_V3);
564  if (unaccountedFlags != 0) {
565  // Newer protocol (unknown flag present) - we will try to continue
566  // since connection has not been refused earlier
567  static bool warnedOnceForward = false;
568  if (!warnedOnceForward) {
569  LOG_WARN("Warning: forward-compatible mode; will attempt to process image stream with unknown extra flags. Consider upgrading the client software.");
570  warnedOnceForward = true;
571  }
572  }
573 
574  imageSet.setWidth(receiveHeader.width);
575  imageSet.setHeight(receiveHeader.height);
576  imageSet.setPixelFormat(0, static_cast<ImageSet::ImageFormat>(receiveHeader.format0));
577  if (imageSet.getNumberOfImages() > 1) imageSet.setPixelFormat(1, static_cast<ImageSet::ImageFormat>(receiveHeader.format1));
578  if (imageSet.getNumberOfImages() > 2) imageSet.setPixelFormat(2, static_cast<ImageSet::ImageFormat>(receiveHeader.format2));
579 
580  int rowStrideArr[ImageSet::MAX_SUPPORTED_IMAGES] = {0};
581  int validRowsArr[ImageSet::MAX_SUPPORTED_IMAGES] = {0};
582  unsigned char* pixelArr[ImageSet::MAX_SUPPORTED_IMAGES] = {nullptr};
583 
584  if (isInterleaved) {
585  // OLD transfer (forced to interleaved 2 images mode)
586  static bool warnedOnceBackward = false;
587  if (!warnedOnceBackward) {
588  LOG_WARN("Info: backward-compatible mode; the device is sending with a legacy protocol. Consider upgrading its firmware.");
589  warnedOnceBackward = true;
590  }
591  unsigned char* data = dataProt.getBlockReceiveBuffer(0);
592  int validBytes = dataProt.getBlockValidSize(0);
593  for (int i=0; i < 2; ++i) {
594  pixelArr[i] = decodeInterleaved(i, imageSet.getNumberOfImages(), validBytes, data, validRowsArr[i], rowStrideArr[i]);
595  }
596  // Legacy sender with mode-dependent channel selection
597  imageSet.setIndexOf(ImageSet::ImageType::IMAGE_LEFT, 0);
598  imageSet.setIndexOf(ImageSet::ImageType::IMAGE_RIGHT, flaggedDisparityPair ? -1 : 1);
599  imageSet.setIndexOf(ImageSet::ImageType::IMAGE_DISPARITY, flaggedDisparityPair ? 1 : -1);
600  } else {
601  // NEW transfer
602  for (int i=0; i<receiveHeader.numberOfImages; ++i) {
603  unsigned char* data = dataProt.getBlockReceiveBuffer(i);
604  int validBytes = dataProt.getBlockValidSize(i);
605  pixelArr[i] = decodeNoninterleaved(i, imageSet.getNumberOfImages(), validBytes, data, validRowsArr[i], rowStrideArr[i]);
606  }
607  if (arbitraryChannels) {
608  // Completely customizable channel selection
609  imageSet.setIndexOf(ImageSet::ImageType::IMAGE_LEFT, -1);
610  imageSet.setIndexOf(ImageSet::ImageType::IMAGE_RIGHT, -1);
611  imageSet.setIndexOf(ImageSet::ImageType::IMAGE_DISPARITY, -1);
612  for (int i=0; i<imageSet.getNumberOfImages(); ++i) {
613  int typ = receiveHeader.imageTypes[i];
614  ImageSet::ImageType imgtype = static_cast<ImageSet::ImageType>(typ);
615  imageSet.setIndexOf(imgtype, i);
616  }
617  } else {
618  static bool warnedOnceV2 = false;
619  if (!warnedOnceV2) {
620  LOG_WARN("Info: received a transfer with header v2");
621  warnedOnceV2 = true;
622  }
623  // Older v2 header; accessing imageTypes is not valid
624  // Two-image sender with mode-dependent channel selection
625  imageSet.setIndexOf(ImageSet::ImageType::IMAGE_LEFT, 0);
626  imageSet.setIndexOf(ImageSet::ImageType::IMAGE_RIGHT, flaggedDisparityPair ? -1 : 1);
627  imageSet.setIndexOf(ImageSet::ImageType::IMAGE_DISPARITY, flaggedDisparityPair ? 1 : -1);
628  }
629  }
630 
631  for (int i=0; i<receiveHeader.numberOfImages; ++i) {
632  imageSet.setRowStride(i, rowStrideArr[i]);
633  imageSet.setPixelData(i, pixelArr[i]);
634  }
635  imageSet.setQMatrix(receiveHeader.q);
636 
637  imageSet.setSequenceNumber(receiveHeader.seqNum);
638  imageSet.setTimestamp(receiveHeader.timeSec, receiveHeader.timeMicrosec);
639  imageSet.setDisparityRange(receiveHeader.minDisparity, receiveHeader.maxDisparity);
640  imageSet.setSubpixelFactor(receiveHeader.subpixelFactor);
641 
642  validRows = validRowsArr[0];
643  for (int i=0; i<receiveHeader.numberOfImages; ++i) {
644  if (validRowsArr[i] < validRows) {
645  validRows = validRowsArr[i];
646  }
647  }
648 
649  if(validRows == receiveHeader.height || receptionDone) {
650  complete = true;
651  resetReception();
652  }
653 
654  return true;
655  }
656 }
657 
658 unsigned char* ImageProtocol::Pimpl::decodeNoninterleaved(int imageNumber, int numImages, int receivedBytes,
659  unsigned char* data, int& validRows, int& rowStride) {
660  ImageSet::ImageFormat format;
661  int bits = 8;
662  switch (imageNumber) {
663  case 0: {
664  format = static_cast<ImageSet::ImageFormat>(receiveHeader.format0);
665  break;
666  }
667  case 1: {
668  format = static_cast<ImageSet::ImageFormat>(receiveHeader.format1);
669  break;
670  }
671  case 2: {
672  format = static_cast<ImageSet::ImageFormat>(receiveHeader.format2);
673  break;
674  }
675  default:
676  throw ProtocolException("Not implemented: decodeNoninterleaved with image index > 2");
677  }
678  bits = getFormatBits(static_cast<ImageSet::ImageFormat>(format), false);
679 
680  int totalBits = bits;
681  unsigned char* ret = nullptr;
682 
683  if(receiveHeader.lastTileWidth == 0) {
684  int bufferOffset0 = 0;
685  int bufferRowStride = receiveHeader.width*(totalBits) / 8;
686 
687  if(format == ImageSet::FORMAT_8_BIT_MONO || format == ImageSet::FORMAT_8_BIT_RGB) {
688  // No decoding is necessary. We can just pass through the
689  // data pointer
690  ret = &data[bufferOffset0];
691  rowStride = bufferRowStride;
692  validRows = receivedBytes / bufferRowStride;
693  } else {
694  // Perform 12-bit => 16 bit decoding
695  allocateDecodeBuffer(imageNumber);
696  validRows = receivedBytes / bufferRowStride;
697  rowStride = 2*receiveHeader.width;
698  int lastRow = lastReceivedPayloadBytes[imageNumber] / bufferRowStride;
699 
700  BitConversions::decode12BitPacked(lastRow, validRows, &data[bufferOffset0],
701  &decodeBuffer[imageNumber][0], bufferRowStride, rowStride, receiveHeader.width);
702 
703  ret = &decodeBuffer[imageNumber][0];
704  }
705  } else {
706  // Decode the tiled transfer
707  decodeTiledImage(imageNumber,
708  lastReceivedPayloadBytes[imageNumber], receivedBytes, data,
709  receiveHeader.firstTileWidth * (totalBits) / 8,
710  receiveHeader.middleTilesWidth * (totalBits) / 8,
711  receiveHeader.lastTileWidth * (totalBits) / 8,
712  validRows, format, false);
713  ret = &decodeBuffer[imageNumber][0];
714  rowStride = receiveHeader.width*getFormatBits(
715  static_cast<ImageSet::ImageFormat>(format), true)/8;
716  }
717 
718  lastReceivedPayloadBytes[imageNumber] = receivedBytes;
719  return ret;
720 }
721 
722 
723 unsigned char* ImageProtocol::Pimpl::decodeInterleaved(int imageNumber, int numImages, int receivedBytes,
724  unsigned char* data, int& validRows, int& rowStride) {
725  ImageSet::ImageFormat format = static_cast<ImageSet::ImageFormat>(
726  imageNumber == 0 ? receiveHeader.format0 : receiveHeader.format1);
727  int bits0 = getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format0), false);
728  int bits1 = getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format1), false);
729  int bits2 = getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format2), false);
730 
731  int totalBits = (numImages<3)?(bits0 + bits1):(bits0 + bits1 + bits2);
732 
733  unsigned char* ret = nullptr;
734 
735  if(receiveHeader.lastTileWidth == 0) {
736  int bufferOffset;
737  switch (imageNumber) {
738  case 0: { bufferOffset = 0; break; }
739  case 1: { bufferOffset = receiveHeader.width * bits0/8; break; }
740  case 2: { bufferOffset = receiveHeader.width * (bits0 + bits1)/8; break; }
741  default:
742  throw ProtocolException("Not implemented: image index > 2");
743  }
744  int bufferRowStride = receiveHeader.width*(totalBits) / 8;
745 
746  if(format == ImageSet::FORMAT_8_BIT_MONO || format == ImageSet::FORMAT_8_BIT_RGB) {
747  // No decoding is necessary. We can just pass through the
748  // data pointer
749  ret = &data[bufferOffset];
750  rowStride = bufferRowStride;
751  validRows = receivedBytes / bufferRowStride;
752  } else {
753  // Perform 12-bit => 16 bit decoding
754  allocateDecodeBuffer(imageNumber);
755  validRows = receivedBytes / bufferRowStride;
756  rowStride = 2*receiveHeader.width;
757  int lastRow = lastReceivedPayloadBytes[imageNumber] / bufferRowStride;
758 
759  BitConversions::decode12BitPacked(lastRow, validRows, &data[bufferOffset],
760  &decodeBuffer[imageNumber][0], bufferRowStride, rowStride, receiveHeader.width);
761 
762  ret = &decodeBuffer[imageNumber][0];
763  }
764  } else {
765  // Decode the tiled transfer
766  decodeTiledImage(imageNumber,
767  lastReceivedPayloadBytes[imageNumber], receivedBytes, data,
768  receiveHeader.firstTileWidth * (totalBits) / 8,
769  receiveHeader.middleTilesWidth * (totalBits) / 8,
770  receiveHeader.lastTileWidth * (totalBits) / 8,
771  validRows, format, true);
772  ret = &decodeBuffer[imageNumber][0];
773  rowStride = receiveHeader.width*getFormatBits(
774  static_cast<ImageSet::ImageFormat>(format), true)/8;
775  }
776 
777  lastReceivedPayloadBytes[imageNumber] = receivedBytes;
778  return ret;
779 }
780 
781 void ImageProtocol::Pimpl::allocateDecodeBuffer(int imageNumber) {
782  ImageSet::ImageFormat format;
783  switch (imageNumber) {
784  case 0: {
785  format = static_cast<ImageSet::ImageFormat>(receiveHeader.format0);
786  break;
787  }
788  case 1: {
789  format = static_cast<ImageSet::ImageFormat>(receiveHeader.format1);
790  break;
791  }
792  case 2: {
793  format = static_cast<ImageSet::ImageFormat>(receiveHeader.format2);
794  break;
795  }
796  default:
797  throw ProtocolException("Not implemented: allocateDecodeBuffer with image index > 2");
798  }
799  int bitsPerPixel = getFormatBits(format, true);
800  int bufferSize = receiveHeader.width * receiveHeader.height * bitsPerPixel / 8;
801 
802  if(decodeBuffer[imageNumber].size() != static_cast<unsigned int>(bufferSize)) {
803  decodeBuffer[imageNumber].resize(bufferSize);
804  }
805 }
806 
807 void ImageProtocol::Pimpl::decodeTiledImage(int imageNumber, int lastReceivedPayloadBytes, int receivedPayloadBytes,
808  const unsigned char* data, int firstTileStride, int middleTilesStride, int lastTileStride, int& validRows,
809  ImageSet::ImageFormat format, bool dataIsInterleaved) {
810  // Allocate a decoding buffer
811  allocateDecodeBuffer(imageNumber);
812 
813  // Get beginning and end of first tile
814  int numTiles = getNumTiles(receiveHeader.width, receiveHeader.firstTileWidth,
815  receiveHeader.middleTilesWidth, receiveHeader.lastTileWidth);
816  int payloadOffset = 0;
817  int decodeXOffset = 0;
818  int prevTileStrides = 0;
819  for(int i = 0; i < numTiles; i++) {
820  // Get relevant parameters
821  int tileWidth = 0;
822  int tileStride = 0;
823 
824  if(i == 0) {
825  tileStride = firstTileStride;
826  tileWidth = receiveHeader.firstTileWidth;
827  } else if(i == numTiles-1) {
828  tileStride = lastTileStride;
829  tileWidth = receiveHeader.lastTileWidth;
830  } else {
831  tileStride = middleTilesStride;
832  tileWidth = receiveHeader.middleTilesWidth;
833  }
834 
835  int tileStart = std::max(0, (lastReceivedPayloadBytes - payloadOffset) / tileStride);
836  int tileStop = std::min(std::max(0, (receivedPayloadBytes - payloadOffset) / tileStride), (int)receiveHeader.height);
837  int tileOffset;
838  if (dataIsInterleaved) {
839  switch (imageNumber) {
840  case 0: { tileOffset = 0; break; }
841  case 1: { tileOffset = tileWidth * (
842  getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format0), false)
843  )/8; break; }
844  case 2: { tileOffset = tileWidth * (
845  getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format0), false)
846  + getFormatBits(static_cast<ImageSet::ImageFormat>(receiveHeader.format1), false)
847  )/8; break; }
848  default:
849  throw ProtocolException("Not implemented: image index > 2");
850  }
851  } else {
852  tileOffset = 0;
853  }
854  if(i > 0) {
855  tileOffset += receiveHeader.height * prevTileStrides;
856  }
857 
858  // Decode
859  int bytesPixel;
860  if(format == ImageSet::FORMAT_12_BIT_MONO) {
861  bytesPixel = 2;
862  BitConversions::decode12BitPacked(tileStart, tileStop, &data[tileOffset],
863  &decodeBuffer[imageNumber][decodeXOffset], tileStride, 2*receiveHeader.width, tileWidth);
864  } else {
865  bytesPixel = (format == ImageSet::FORMAT_8_BIT_RGB ? 3 : 1);
866  decodeRowsFromTile(tileStart, tileStop, &data[tileOffset],
867  &decodeBuffer[imageNumber][decodeXOffset], tileStride,
868  receiveHeader.width*bytesPixel, tileWidth*bytesPixel);
869  }
870 
871  payloadOffset += receiveHeader.height * tileStride;
872  decodeXOffset += tileWidth * bytesPixel;
873  prevTileStrides += tileStride;
874  if(i == numTiles-1) {
875  validRows = tileStop;
876  }
877  }
878 }
879 
880 void ImageProtocol::Pimpl::decodeRowsFromTile(int startRow, int stopRow, unsigned const char* src,
881  unsigned char* dst, int srcStride, int dstStride, int tileWidth) {
882  for(int y = startRow; y < stopRow; y++) {
883  memcpy(&dst[y*dstStride], &src[y*srcStride], tileWidth);
884  }
885 }
886 
887 void ImageProtocol::Pimpl::resetReception() {
888  receiveHeaderParsed = false;
889  for (int i=0; i<ImageSet::MAX_SUPPORTED_IMAGES; ++i) {
890  lastReceivedPayloadBytes[i] = 0;
891  }
892  dataProt.resetReception(false);
893  receptionDone = false;
894 }
895 
896 bool ImageProtocol::Pimpl::isConnected() const {
897  return dataProt.isConnected();
898 }
899 
900 const unsigned char* ImageProtocol::Pimpl::getNextControlMessage(int& length) {
901  return dataProt.getNextControlMessage(length);
902 }
903 
904 bool ImageProtocol::Pimpl::newClientConnected() {
905  return dataProt.newClientConnected();
906 }
907 
908 int ImageProtocol::Pimpl::getNumDroppedFrames() const {
909  return dataProt.getDroppedReceptions();
910 }
911 
912 std::string ImageProtocol::statusReport() {
913  return pimpl->statusReport();
914 }
915 std::string ImageProtocol::Pimpl::statusReport() {
916  return dataProt.statusReport();
917 }
918 
919 
920 
921 } // namespace
922 
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.
Definition: imageset.h:407
int getRowStride(int imageNumber) const
Returns the row stride for the pixel data of one image.
Definition: imageset.h:216
void setTimestamp(int seconds, int microsec)
Sets the time at which this image set has been captured.
Definition: imageset.h:163
void setPixelData(int imageNumber, unsigned char *pixelData)
Sets the pixel data for the given image.
Definition: imageset.h:134
void getTimestamp(int &seconds, int &microsec) const
Returns the time at which this image set has been captured.
Definition: imageset.h:307
void processReceivedMessage(int length)
Handles a received network message.
const float * getQMatrix() const
Returns a pointer to the disparity-to-depth mapping matrix q.
Definition: imageset.h:291
void setWidth(int w)
Sets a new width for both images.
Definition: imageset.h:90
int getBitsPerPixel(int imageNumber) const
Returns the number of bits that are required to store one image pixel.
Definition: imageset.h:379
int getIndexOf(ImageType what, bool throwIfNotFound=false) const
Returns the index of a specific image type.
Definition: imageset.cpp:208
int getNumberOfImages() const
Returns the number of images in this set.
Definition: imageset.h:400
unsigned int getSequenceNumber() const
Returns the sequence number for this image set.
Definition: imageset.h:298
void setSequenceNumber(unsigned int num)
Sets the sequence number for this image set.
Definition: imageset.h:152
int getHeight() const
Returns the height of each image.
Definition: imageset.h:205
void setHeight(int h)
Sets a new width for both images.
Definition: imageset.h:95
int getBytesPerPixel(int imageNumber) const
Returns the number of bytes that are required to store one image pixel.
Definition: imageset.h:368
bool isConnected() const
Returns true if a remote connection is established.
int getSubpixelFactor() const
Gets the subpixel factor for this image set.
Definition: imageset.h:328
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.
Definition: imageset.h:116
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.
Definition: imageprotocol.h:43
void setRowStride(int imageNumber, int stride)
Sets a new row stride for the pixel data of one image.
Definition: imageset.h:104
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...
Definition: imageset.h:320
ImageType
Supported image types.
Definition: imageset.h:67
ImageFormat
Image formats that can be transferred.
Definition: imageset.h:44
bool getReceivedImageSet(ImageSet &imageSet)
Returns a received image when complete.
unsigned char * getPixelData(int imageNumber) const
Returns the pixel data for the given image.
Definition: imageset.h:270
A set of one to three images, but usually two (the left camera image and the disparity map)...
Definition: imageset.h:38
ImageFormat getPixelFormat(int imageNumber) const
Returns the pixel format for the given image.
Definition: imageset.h:243
const unsigned char * getTransferMessage(int &length)
Gets the next network message for the current transfer.
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.
Definition: imageset.cpp:230
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.
Definition: imageset.h:175
Exception class that is used for all protocol exceptions.
Definition: exceptions.h:25
int getWidth() const
Returns the width of each image.
Definition: imageset.h:200
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.
Definition: imageset.h:183
void setQMatrix(const float *q)
Sets the pointer to the disparity-to-depth mapping matrix q.
Definition: imageset.h:145
Nerian Vision Technologies