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" 34 #include <arpa/inet.h> 41 class ImageProtocol::Pimpl {
43 Pimpl(ProtocolType protType,
int maxUdpPacketSize);
46 void setTransferImagePair(
const ImagePair& imagePair);
47 void setRawTransferData(
const ImagePair& metaData,
unsigned char* rawData,
48 int firstTileWidth = 0,
int secondTileWidth = 0,
int validBytes = 0x7FFFFFFF);
49 void setRawValidBytes(
int validBytes);
50 const unsigned char* getTransferMessage(
int& length);
51 bool transferComplete();
53 bool getReceivedImagePair(
ImagePair& imagePair);
54 bool getPartiallyReceivedImagePair(
ImagePair& imagePair,
55 int& validRows,
bool& complete);
56 bool imagesReceived()
const;
58 unsigned char* getNextReceiveBuffer(
int& maxLength);
60 bool processReceivedMessage(
int length);
61 int getProspectiveMessageSize();
62 void resetReception();
65 enum HeaderPixelFormat {
68 HDR_FMT_12_BIT_PACKED,
74 unsigned char protocolVersion;
75 unsigned char isRawImagePair;
78 unsigned short height;
80 unsigned short firstTileWidth;
81 unsigned short secondTileWidth;
83 unsigned char format0;
84 unsigned char format1;
85 unsigned char minDisparity;
86 unsigned char maxDisparity;
94 unsigned char padding1[6];
98 static const unsigned char CURRENT_VERSION = 0x04;
102 ProtocolType protType;
105 bool headerTransferred;
106 std::vector<unsigned char> headerBuffer;
107 std::vector<unsigned char> rawBuffer;
108 unsigned char* rawData;
113 std::vector<unsigned char, AlignedAllocator<unsigned char> >decodeBuffer[2];
114 bool receiveHeaderParsed;
115 HeaderData receiveHeader;
116 int lastReceivedPayloadBytes[2];
117 int receiveTotalSize;
121 void copyHeaderToBuffer(
const ImagePair& imagePair,
int firstTileWidth,
122 int secondTileWidth,
unsigned char* buffer);
125 void tryDecodeHeader(
const unsigned char* receivedData,
int receivedBytes);
128 unsigned char* decodeInterleaved(
int imageNumber,
int receivedBytes,
129 unsigned char* data,
int& validRows,
int& rowStride);
131 int getFrameSize(
int width,
int height,
int firstTileWidth,
int secondTileWidth,
134 int getFormatNibbles(HeaderPixelFormat format);
136 void decodeTiledImage(
int imageNumber,
int lastReceivedPayloadBytes,
int receivedPayloadBytes,
137 const unsigned char* data,
int firstTileStride,
int secondTileStride,
int& validRows,
138 HeaderPixelFormat format);
140 void decodeRowsFromTile(
int startRow,
int stopRow,
unsigned const char* src,
141 unsigned char* dst,
int srcStride,
int dstStride,
int tileWidth);
143 void allocateDecodeBuffer(
int imageNumber);
150 : pimpl(new Pimpl(protType, maxUdpPacketSize)) {
154 ImageProtocol::~ImageProtocol() {
159 pimpl->setTransferImagePair(imagePair);
163 unsigned char* imageData,
int firstTileWidth,
int secondTileWidth,
int validBytes) {
164 pimpl->setRawTransferData(metaData, imageData, firstTileWidth, secondTileWidth, validBytes);
168 pimpl->setRawValidBytes(validBytes);
172 return pimpl->getTransferMessage(length);
176 return pimpl->transferComplete();
180 pimpl->resetTransfer();
184 return pimpl->getReceivedImagePair(imagePair);
188 ImagePair& imagePair,
int& validRows,
bool& complete) {
189 return pimpl->getPartiallyReceivedImagePair(imagePair, validRows, complete);
193 return pimpl->imagesReceived();
197 return pimpl->getNextReceiveBuffer(maxLength);
201 return pimpl->processReceivedMessage(length);
205 pimpl->resetReception();
210 ImageProtocol::Pimpl::Pimpl(
ProtocolType protType,
int maxUdpPacketSize)
211 :dataProt(static_cast<DataBlockProtocol::ProtocolType>(protType), maxUdpPacketSize),
212 protType(protType), headerTransferred(
false),
213 rawData(
nullptr), rawValidBytes(0), rawDataLength(0), receiveHeaderParsed(
false),
214 lastReceivedPayloadBytes{0, 0}, receiveTotalSize(0), receptionDone(
false) {
215 headerBuffer.resize(
sizeof(HeaderData) +
sizeof(
unsigned short));
216 memset(&headerBuffer[0], 0,
sizeof(headerBuffer.size()));
217 memset(&receiveHeader, 0,
sizeof(receiveHeader));
221 dataProt.setReceiveDataSize(maxUdpPacketSize);
224 void ImageProtocol::Pimpl::setTransferImagePair(
const ImagePair& imagePair) {
229 headerTransferred =
false;
232 copyHeaderToBuffer(imagePair, 0, 0, &headerBuffer[0]);
233 dataProt.startTransfer();
234 dataProt.setTransferData(&headerBuffer[0],
sizeof(HeaderData));
240 rawBuffer.resize(imagePair.
getWidth()*imagePair.
getHeight()*(bytes0 + bytes1) +
sizeof(
short));
242 int bufferOffset = 0;
243 int row0Size = imagePair.
getWidth()*bytes0;
244 int row1Size = imagePair.
getWidth()*bytes1;
245 for(
int y = 0; y<imagePair.
getHeight(); y++) {
247 bufferOffset += row0Size;
250 bufferOffset += row1Size;
253 rawData = &rawBuffer[0];
254 rawValidBytes =
static_cast<int>(rawBuffer.size() -
sizeof(short));
260 void ImageProtocol::Pimpl::setRawTransferData(
const ImagePair& metaData,
unsigned char* rawData,
261 int firstTileWidth,
int secondTileWidth,
int validBytes) {
262 if(rawData ==
nullptr) {
266 headerTransferred =
false;
269 copyHeaderToBuffer(metaData, firstTileWidth, secondTileWidth, &headerBuffer[0]);
270 dataProt.startTransfer();
271 dataProt.setTransferData(&headerBuffer[0],
sizeof(HeaderData));
273 this->rawData = rawData;
274 rawValidBytes = validBytes;
281 void ImageProtocol::Pimpl::setRawValidBytes(
int validBytes) {
282 rawValidBytes = validBytes;
283 if(headerTransferred) {
284 dataProt.setTransferValidBytes(validBytes);
288 const unsigned char* ImageProtocol::Pimpl::getTransferMessage(
int& length) {
289 const unsigned char* msg = dataProt.getTransferMessage(length);
291 if(msg ==
nullptr && !headerTransferred && rawValidBytes > 0) {
294 headerTransferred =
true;
295 dataProt.setTransferData(rawData, rawDataLength, rawValidBytes);
296 msg = dataProt.getTransferMessage(length);
302 bool ImageProtocol::Pimpl::transferComplete() {
303 return dataProt.transferComplete() && headerTransferred;
306 int ImageProtocol::Pimpl::getFrameSize(
int width,
int height,
int firstTileWidth,
312 int effectiveWidth = firstTileWidth > 0 ? firstTileWidth + secondTileWidth : width;
314 return (effectiveWidth * height * (nibbles0 + nibbles1)) /2 + headerSize;
317 int ImageProtocol::Pimpl::getFormatNibbles(HeaderPixelFormat format) {
319 if(format == HDR_FMT_12_BIT_SPLIT || format == HDR_FMT_12_BIT_PACKED) {
326 void ImageProtocol::Pimpl::copyHeaderToBuffer(
const ImagePair& imagePair,
327 int firstTileWidth,
int secondTileWidth,
unsigned char* buffer) {
328 HeaderData* transferHeader =
reinterpret_cast<HeaderData*
>(buffer);
329 memset(transferHeader, 0,
sizeof(*transferHeader));
330 transferHeader->protocolVersion = CURRENT_VERSION;
332 transferHeader->width = htons(imagePair.
getWidth());
333 transferHeader->height = htons(imagePair.
getHeight());
334 transferHeader->firstTileWidth = htons(firstTileWidth);
335 transferHeader->secondTileWidth = htons(secondTileWidth);
338 transferHeader->seqNum =
static_cast<unsigned int>(htonl(imagePair.
getSequenceNumber()));
340 int minDisp = 0, maxDisp = 0;
342 transferHeader->minDisparity = minDisp;
343 transferHeader->maxDisparity = maxDisp;
345 int timeSec = 0, timeMicrosec = 0;
347 transferHeader->timeSec =
static_cast<int>(htonl(static_cast<unsigned int>(timeSec)));
348 transferHeader->timeMicrosec =
static_cast<int>(htonl(static_cast<unsigned int>(timeMicrosec)));
351 memcpy(transferHeader->q, imagePair.
getQMatrix(),
sizeof(float)*16);
355 void ImageProtocol::Pimpl::resetTransfer() {
356 dataProt.resetTransfer();
359 unsigned char* ImageProtocol::Pimpl::getNextReceiveBuffer(
int& maxLength) {
360 maxLength = dataProt.getMaxReceptionSize();
361 return dataProt.getNextReceiveBuffer(maxLength);
364 bool ImageProtocol::Pimpl::processReceivedMessage(
int length) {
365 receptionDone =
false;
368 if(!dataProt.processReceivedMessage(length)) {
373 int receivedBytes = 0;
374 const unsigned char* receivedData = dataProt.getReceivedData(receivedBytes);
377 if(!receiveHeaderParsed && receivedBytes > 0) {
378 tryDecodeHeader(receivedData, receivedBytes);
382 if(receivedBytes == receiveTotalSize) {
383 receptionDone =
true;
384 dataProt.finishReception();
385 }
else if(receivedBytes > receiveTotalSize) {
387 dataProt.resetReception();
394 void ImageProtocol::Pimpl::tryDecodeHeader(
const 395 unsigned char* receivedData,
int receivedBytes) {
396 if(receivedBytes >= static_cast<int>(
sizeof(HeaderData))) {
397 receiveHeader = *
reinterpret_cast<const HeaderData*
>(receivedData);
399 if(receiveHeader.protocolVersion > CURRENT_VERSION ||
400 receiveHeader.protocolVersion < 4) {
405 receiveHeader.width = ntohs(receiveHeader.width);
406 receiveHeader.height = ntohs(receiveHeader.height);
407 receiveHeader.firstTileWidth = ntohs(receiveHeader.firstTileWidth);
408 receiveHeader.secondTileWidth = ntohs(receiveHeader.secondTileWidth);
409 receiveHeader.timeSec =
static_cast<int>(
410 htonl(static_cast<unsigned int>(receiveHeader.timeSec)));
411 receiveHeader.timeMicrosec =
static_cast<int>(
412 htonl(static_cast<unsigned int>(receiveHeader.timeMicrosec)));
413 receiveHeader.seqNum = htonl(receiveHeader.seqNum);
416 receiveTotalSize = getFrameSize(
418 receiveHeader.height,
419 receiveHeader.firstTileWidth,
420 receiveHeader.secondTileWidth,
425 dataProt.setReceiveDataSize(receiveTotalSize);
426 receiveHeaderParsed =
true;
430 bool ImageProtocol::Pimpl::imagesReceived()
const {
431 return receptionDone && receiveHeaderParsed;
434 bool ImageProtocol::Pimpl::getReceivedImagePair(
ImagePair& imagePair) {
435 bool complete =
false;
439 return (ok && complete);
442 bool ImageProtocol::Pimpl::getPartiallyReceivedImagePair(
ImagePair& imagePair,
int& validRows,
bool& complete) {
448 if(!receiveHeaderParsed) {
453 int receivedBytes = 0;
454 unsigned char* data = dataProt.getReceivedData(receivedBytes);
455 if(receivedBytes == receiveTotalSize) {
457 dataProt.finishReception();
463 imagePair.
setWidth(receiveHeader.width);
464 imagePair.
setHeight(receiveHeader.height);
468 int rowStride0 = 0, rowStride1 = 0;
469 int validRows0 = 0, validRows1 = 0;
470 unsigned char* pixel0 = decodeInterleaved(0, receivedBytes, data, validRows0, rowStride0);
471 unsigned char* pixel1 = decodeInterleaved(1, receivedBytes, data, validRows1, rowStride1);
480 imagePair.
setTimestamp(receiveHeader.timeSec, receiveHeader.timeMicrosec);
481 imagePair.
setDisparityRange(receiveHeader.minDisparity, receiveHeader.maxDisparity);
483 validRows = min(validRows0, validRows1);
485 if(validRows == receiveHeader.height) {
498 unsigned char* ImageProtocol::Pimpl::decodeInterleaved(
int imageNumber,
int receivedBytes,
499 unsigned char* data,
int& validRows,
int& rowStride) {
500 if(receivedBytes <= static_cast<int>(
sizeof(HeaderData))) {
505 HeaderPixelFormat format =
static_cast<HeaderPixelFormat
>(
506 imageNumber == 0 ? receiveHeader.format0 : receiveHeader.format1);
507 int nibbles0 = getFormatNibbles(static_cast<HeaderPixelFormat>(receiveHeader.format0));
508 int nibbles1 = getFormatNibbles(static_cast<HeaderPixelFormat>(receiveHeader.format1));
510 unsigned char* ret =
nullptr;
511 int payloadBytes = receivedBytes -
sizeof(HeaderData);
513 if(receiveHeader.secondTileWidth == 0) {
514 int bufferOffset =
sizeof(HeaderData) + imageNumber*receiveHeader.width * nibbles0/2;
515 int bufferRowStride = receiveHeader.width*(nibbles0 + nibbles1) / 2;
517 if(format == HDR_FMT_8_BIT) {
520 ret = &data[bufferOffset];
521 rowStride = bufferRowStride;
522 validRows = payloadBytes / bufferRowStride;
525 allocateDecodeBuffer(imageNumber);
526 validRows = payloadBytes / bufferRowStride;
527 rowStride = 2*receiveHeader.width;
528 int lastRow = lastReceivedPayloadBytes[imageNumber] / bufferRowStride;
530 if(format == HDR_FMT_12_BIT_SPLIT) {
531 BitConversions::decode12BitSplit(lastRow, validRows, &data[bufferOffset],
532 &decodeBuffer[imageNumber][0], bufferRowStride, rowStride, receiveHeader.width);
534 BitConversions::decode12BitPacked(lastRow, validRows, &data[bufferOffset],
535 &decodeBuffer[imageNumber][0], bufferRowStride, rowStride, receiveHeader.width);
538 ret = &decodeBuffer[imageNumber][0];
542 decodeTiledImage(imageNumber,
543 lastReceivedPayloadBytes[imageNumber], payloadBytes,
544 data, receiveHeader.firstTileWidth * (nibbles0 + nibbles1) / 2,
545 receiveHeader.secondTileWidth * (nibbles0 + nibbles1) / 2,
547 ret = &decodeBuffer[imageNumber][0];
549 if(format == HDR_FMT_8_BIT) {
550 rowStride = receiveHeader.width;
552 rowStride = 2*receiveHeader.width;
556 lastReceivedPayloadBytes[imageNumber] = payloadBytes;
560 void ImageProtocol::Pimpl::allocateDecodeBuffer(
int imageNumber) {
561 HeaderPixelFormat format =
static_cast<HeaderPixelFormat
>(
562 imageNumber == 0 ? receiveHeader.format0 : receiveHeader.format1);
563 int bytesPerPixel = (format == HDR_FMT_8_BIT ? 1 : 2);
564 int bufferSize = receiveHeader.width * receiveHeader.height * bytesPerPixel;
566 if(decodeBuffer[imageNumber].size() !=
static_cast<unsigned int>(bufferSize)) {
567 decodeBuffer[imageNumber].resize(bufferSize);
571 void ImageProtocol::Pimpl::decodeTiledImage(
int imageNumber,
int lastReceivedPayloadBytes,
int receivedPayloadBytes,
572 const unsigned char* data,
int firstTileStride,
int secondTileStride,
int& validRows,
573 HeaderPixelFormat format) {
576 allocateDecodeBuffer(imageNumber);
579 int startFirstTile = lastReceivedPayloadBytes / firstTileStride;
580 int stopFirstTile = std::min(receivedPayloadBytes / firstTileStride,
581 static_cast<int>(receiveHeader.height));
584 int secondTileBytes = receivedPayloadBytes - (receiveHeader.height*firstTileStride);
585 int lastSecondTileBytes = lastReceivedPayloadBytes - (receiveHeader.height*firstTileStride);
586 int startSecondTile = std::max(0, lastSecondTileBytes / secondTileStride);
587 int stopSecondTile = std::max(0, secondTileBytes / secondTileStride);
588 int firstTileOffset =
sizeof(HeaderData) + imageNumber * getFormatNibbles(
589 static_cast<HeaderPixelFormat>(receiveHeader.format0)) * receiveHeader.firstTileWidth / 2;
592 if(format == HDR_FMT_12_BIT_SPLIT) {
593 BitConversions::decode12BitSplit(startFirstTile, stopFirstTile, &data[firstTileOffset], &decodeBuffer[imageNumber][0],
594 firstTileStride, 2*receiveHeader.width, receiveHeader.firstTileWidth);
595 }
else if(format == HDR_FMT_12_BIT_PACKED) {
596 BitConversions::decode12BitPacked(startFirstTile, stopFirstTile, &data[firstTileOffset], &decodeBuffer[imageNumber][0],
597 firstTileStride, 2*receiveHeader.width, receiveHeader.firstTileWidth);
599 decodeRowsFromTile(startFirstTile, stopFirstTile, &data[firstTileOffset],
600 &decodeBuffer[imageNumber][0], firstTileStride, receiveHeader.width,
601 receiveHeader.firstTileWidth);
605 int secondTileOffset =
sizeof(HeaderData) + receiveHeader.height*firstTileStride +
606 imageNumber * getFormatNibbles(static_cast<HeaderPixelFormat>(receiveHeader.format0)) * receiveHeader.secondTileWidth / 2;
608 if(format == HDR_FMT_12_BIT_SPLIT) {
609 BitConversions::decode12BitSplit(startSecondTile, stopSecondTile,
610 &data[secondTileOffset], &decodeBuffer[imageNumber][2*receiveHeader.firstTileWidth],
611 secondTileStride, 2*receiveHeader.width, receiveHeader.secondTileWidth);
612 }
else if(format == HDR_FMT_12_BIT_PACKED) {
613 BitConversions::decode12BitPacked(startSecondTile, stopSecondTile,
614 &data[secondTileOffset], &decodeBuffer[imageNumber][2*receiveHeader.firstTileWidth],
615 secondTileStride, 2*receiveHeader.width, receiveHeader.secondTileWidth);
617 decodeRowsFromTile(startSecondTile, stopSecondTile, &data[secondTileOffset],
618 &decodeBuffer[imageNumber][receiveHeader.firstTileWidth],
619 secondTileStride, receiveHeader.width, receiveHeader.secondTileWidth);
622 validRows = stopSecondTile;
625 void ImageProtocol::Pimpl::decodeRowsFromTile(
int startRow,
int stopRow,
unsigned const char* src,
626 unsigned char* dst,
int srcStride,
int dstStride,
int tileWidth) {
627 for(
int y = startRow; y < stopRow; y++) {
628 memcpy(&dst[y*dstStride], &src[y*srcStride], tileWidth);
632 void ImageProtocol::Pimpl::resetReception() {
633 receiveHeaderParsed =
false;
634 lastReceivedPayloadBytes[0] = 0;
635 lastReceivedPayloadBytes[1] = 0;
636 receiveTotalSize = 0;
637 dataProt.resetReception();
638 receptionDone =
false;
bool isImageDisparityPair() const
Returns true if this is a left camera image and disparity map pair.
void setTimestamp(int seconds, int microsec)
Sets the time at which this image pair has been captured.
void setHeight(int h)
Sets a new width for both images.
Exception class that is used for all protocol exceptions.
unsigned char * getNextReceiveBuffer(int &maxLength)
Returns the buffer for receiving the next network message.
void setSequenceNumber(unsigned int num)
Sets the sequence number for this image pair.
void setPixelFormat(int imageNumber, ImageFormat format)
Sets the pixel format for the given image.
bool getReceivedImagePair(ImagePair &imagePair)
Returns a received image when complete.
void resetTransfer()
Aborts the transmission of the current transfer and performs a reset of the internal state...
unsigned char * getPixelData(int imageNumber) const
Returns the pixel data for the given image.
bool transferComplete()
Returns true if the current transfer has been completed.
A protocol for transmitting large blocks of data over a network.
const float * getQMatrix() const
Returns a pointer to the disparity-to-depth mapping matrix q.
int getWidth() const
Returns the width of each image.
void setDisparityRange(int minimum, int maximum)
Sets the value range for the disparity map contained in this image pair.
void setWidth(int w)
Sets a new width for both images.
void setQMatrix(const float *q)
Sets the pointer to the disparity-to-depth mapping matrix q.
void setRowStride(int imageNumber, int stride)
Sets a new row stride for the pixel data of one image.
ImageProtocol(ProtocolType protType, int maxUdpPacketSize=1472)
Creates a new instance for decoding / encoding network messages for the given network protocol...
void setTransferImagePair(const ImagePair &imagePair)
Sets a new image that will be transfer.
bool processReceivedMessage(int length)
Handles a received network message.
void setRawValidBytes(int validBytes)
Updates the number of valid bytes in a partial raw transfer.
void getTimestamp(int &seconds, int µsec) const
Returns the time at which this image pair has been captured.
void setPixelData(int imageNumber, unsigned char *pixelData)
Sets the pixel data for the given image.
void getDisparityRange(int &minimum, int &maximum) const
Gets the value range for the disparity map contained in this image pair. If the image pair does not c...
ProtocolType
Supported network protocols.
void resetReception()
Aborts the reception of the current image transfer and resets the internal state. ...
A set of two images, which are usually the left camera image and the disparity map.
ImageFormat getPixelFormat(int imageNumber) const
Returns the pixel format for the given image.
const unsigned char * getTransferMessage(int &length)
Gets the next network message for the current transfer.
void setRawTransferData(const ImagePair &metaData, unsigned char *rawData, int firstTileWidth=0, int secondTileWidth=0, int validBytes=0x7FFFFFFF)
Sets the already pre-formatted image data for the next transfer.
int getHeight() const
Returns the height of each image.
unsigned int getSequenceNumber() const
Returns the sequence number for this image pair.
int getRowStride(int imageNumber) const
Returns the row stride for the pixel data of one image.
void setImageDisparityPair(bool dispPair)
Sets whether this is a left camera image and disparity map pair, or two raw camera images...
ImageFormat
Image formats that can be transferred.
bool imagesReceived() const
Returns true if the images of the current transfer have been received.
bool getPartiallyReceivedImagePair(ImagePair &imagePair, int &validRows, bool &complete)
Returns a partially received image.