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