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" 35 #include <arpa/inet.h> 46 class ImageProtocol::Pimpl {
48 Pimpl(
bool server, ProtocolType protType,
int maxUdpPacketSize);
51 void setTransferImagePair(
const ImagePair& imagePair);
52 void setRawTransferData(
const ImagePair& metaData,
unsigned char* rawData,
53 int firstTileWidth = 0,
int middleTilesWidth = 0,
int lastTileWidth = 0,
int validBytes = 0x7FFFFFFF);
54 void setRawValidBytes(
int validBytes);
55 const unsigned char* getTransferMessage(
int& length);
56 bool transferComplete();
58 bool getReceivedImagePair(
ImagePair& imagePair);
59 bool getPartiallyReceivedImagePair(
ImagePair& imagePair,
60 int& validRows,
bool& complete);
61 bool imagesReceived()
const;
63 unsigned char* getNextReceiveBuffer(
int& maxLength);
65 void processReceivedMessage(
int length);
66 int getProspectiveMessageSize();
67 int getNumDroppedFrames()
const;
68 void resetReception();
69 bool isConnected()
const;
70 const unsigned char* getNextControlMessage(
int& length);
71 bool newClientConnected();
74 unsigned short MAGIC_SEQUECE = 0x3D15;
81 unsigned char protocolVersion;
82 unsigned char isRawImagePair;
85 unsigned short height;
87 unsigned short firstTileWidth;
88 unsigned short lastTileWidth;
90 unsigned char format0;
91 unsigned char format1;
92 unsigned short minDisparity;
93 unsigned short maxDisparity;
94 unsigned char subpixelFactor;
102 unsigned short middleTilesWidth;
108 ProtocolType protType;
111 std::vector<unsigned char> headerBuffer;
112 std::vector<unsigned char> rawBuffer;
113 unsigned char* rawData;
116 std::vector<unsigned char, AlignedAllocator<unsigned char> >decodeBuffer[2];
117 bool receiveHeaderParsed;
118 HeaderData receiveHeader;
119 int lastReceivedPayloadBytes[2];
123 void copyHeaderToBuffer(
const ImagePair& imagePair,
int firstTileWidth,
124 int middleTilesWidth,
int lastTileWidth,
unsigned char* buffer);
127 void tryDecodeHeader(
const unsigned char* receivedData,
int receivedBytes);
130 unsigned char* decodeInterleaved(
int imageNumber,
int receivedBytes,
131 unsigned char* data,
int& validRows,
int& rowStride);
133 int getNumTiles(
int width,
int firstTileWidth,
int middleTilesWidth,
int lastTileWidth);
135 int getFrameSize(
int width,
int height,
int firstTileWidth,
int middleTilesWidth,
140 void decodeTiledImage(
int imageNumber,
int lastReceivedPayloadBytes,
int receivedPayloadBytes,
141 const unsigned char* data,
int firstTileStride,
int middleTilesStride,
int lastTileStride,
144 void decodeRowsFromTile(
int startRow,
int stopRow,
unsigned const char* src,
145 unsigned char* dst,
int srcStride,
int dstStride,
int tileWidth);
147 void allocateDecodeBuffer(
int imageNumber);
153 ImageProtocol::ImageProtocol(
bool server,
ProtocolType protType,
int maxUdpPacketSize)
154 : pimpl(new Pimpl(server, protType, maxUdpPacketSize)) {
158 ImageProtocol::~ImageProtocol() {
163 pimpl->setTransferImagePair(imagePair);
167 unsigned char* imageData,
int firstTileWidth,
int middleTilesWidth,
int lastTileWidth,
int validBytes) {
168 pimpl->setRawTransferData(metaData, imageData, firstTileWidth, middleTilesWidth, lastTileWidth, validBytes);
172 pimpl->setRawValidBytes(validBytes);
176 return pimpl->getTransferMessage(length);
180 return pimpl->transferComplete();
184 pimpl->resetTransfer();
188 return pimpl->getReceivedImagePair(imagePair);
192 ImagePair& imagePair,
int& validRows,
bool& complete) {
193 return pimpl->getPartiallyReceivedImagePair(imagePair, validRows, complete);
197 return pimpl->imagesReceived();
201 return pimpl->getNextReceiveBuffer(maxLength);
205 pimpl->processReceivedMessage(length);
209 return pimpl->getNumDroppedFrames();
213 pimpl->resetReception();
217 return pimpl->isConnected();
221 return pimpl->getNextControlMessage(length);
225 return pimpl->newClientConnected();
230 ImageProtocol::Pimpl::Pimpl(
bool server,
ProtocolType protType,
int maxUdpPacketSize)
231 :dataProt(server, (DataBlockProtocol::ProtocolType)protType,
232 maxUdpPacketSize), protType(protType), rawData(
nullptr),
233 receiveHeaderParsed(
false), lastReceivedPayloadBytes{0, 0},
234 receptionDone(
false) {
235 headerBuffer.resize(
sizeof(HeaderData) + 32);
236 memset(&headerBuffer[0], 0,
sizeof(headerBuffer.size()));
237 memset(&receiveHeader, 0,
sizeof(receiveHeader));
240 void ImageProtocol::Pimpl::setTransferImagePair(
const ImagePair& imagePair) {
246 int rawDataLength = getFrameSize(imagePair.
getWidth(), imagePair.
getHeight(), 0, 0, 0,
250 copyHeaderToBuffer(imagePair, 0, 0, 0, &headerBuffer[16]);
251 dataProt.resetTransfer();
252 dataProt.setTransferHeader(&headerBuffer[16],
sizeof(HeaderData), rawDataLength);
255 int bits[2] = {0, 0};
256 int rowSize[2] = {0, 0};
257 int rowStride[2] = {0, 0};
258 const unsigned char* pixelData[2] = {
nullptr,
nullptr};
259 std::vector<unsigned char> encodingBuffer[2];
261 for(
int i = 0; i<2; i++) {
263 rowSize[i] = imagePair.
getWidth()*bits[i]/8;
269 encodingBuffer[i].resize(rowSize[i] * imagePair.
getHeight());
272 pixelData[i] = &encodingBuffer[i][0];
273 rowStride[i] = rowSize[i];
278 rawBuffer.resize(imagePair.
getWidth()*imagePair.
getHeight()*(bits[0] + bits[1])/8 +
sizeof(
int));
279 int bufferOffset = 0;
281 for(
int y = 0; y<imagePair.
getHeight(); y++) {
282 memcpy(&rawBuffer[bufferOffset], &pixelData[0][y*rowStride[0]], rowSize[0]);
283 bufferOffset += rowSize[0];
285 memcpy(&rawBuffer[bufferOffset], &pixelData[1][y*rowStride[1]], rowSize[1]);
286 bufferOffset += rowSize[1];
289 rawData = &rawBuffer[0];
290 int rawValidBytes =
static_cast<int>(rawBuffer.size() -
sizeof(int));
292 dataProt.setTransferData(rawData, rawValidBytes);
295 void ImageProtocol::Pimpl::setRawTransferData(
const ImagePair& metaData,
unsigned char* rawData,
296 int firstTileWidth,
int middleTilesWidth,
int lastTileWidth,
int validBytes) {
297 if(rawData ==
nullptr) {
303 firstTileWidth, middleTilesWidth, lastTileWidth, metaData.
getPixelFormat(0),
307 copyHeaderToBuffer(metaData, firstTileWidth, middleTilesWidth, lastTileWidth, &headerBuffer[16]);
308 dataProt.resetTransfer();
309 dataProt.setTransferHeader(&headerBuffer[16],
sizeof(HeaderData), rawDataLength);
311 this->rawData = rawData;
313 dataProt.setTransferData(rawData, validBytes);
316 void ImageProtocol::Pimpl::setRawValidBytes(
int validBytes) {
317 dataProt.setTransferValidBytes(validBytes);
320 const unsigned char* ImageProtocol::Pimpl::getTransferMessage(
int& length) {
321 const unsigned char* msg = dataProt.getTransferMessage(length);
324 msg = dataProt.getTransferMessage(length);
330 bool ImageProtocol::Pimpl::transferComplete() {
331 return dataProt.transferComplete();
334 int ImageProtocol::Pimpl::getNumTiles(
int width,
int firstTileWidth,
int middleTilesWidth,
int lastTileWidth) {
335 if(lastTileWidth == 0) {
337 }
else if(middleTilesWidth == 0) {
340 int tileWidth = firstTileWidth + lastTileWidth - middleTilesWidth;
341 return (width - 2*tileWidth + firstTileWidth + lastTileWidth) / (firstTileWidth + lastTileWidth - tileWidth);
345 int ImageProtocol::Pimpl::getFrameSize(
int width,
int height,
int firstTileWidth,
349 int bits0 = getFormatBits(format0,
false);
350 int bits1 = getFormatBits(format1,
false);
351 return (width * height * (bits0 + bits1)) /8;
367 void ImageProtocol::Pimpl::copyHeaderToBuffer(
const ImagePair& imagePair,
368 int firstTileWidth,
int middleTilesWidth,
int lastTileWidth,
unsigned char* buffer) {
369 HeaderData* transferHeader =
reinterpret_cast<HeaderData*
>(buffer);
370 memset(transferHeader, 0,
sizeof(*transferHeader));
371 transferHeader->magic = htons(MAGIC_SEQUECE);
372 transferHeader->protocolVersion = InternalInformation::CURRENT_PROTOCOL_VERSION;
374 transferHeader->width = htons(imagePair.
getWidth());
375 transferHeader->height = htons(imagePair.
getHeight());
376 transferHeader->firstTileWidth = htons(firstTileWidth);
377 transferHeader->lastTileWidth = htons(lastTileWidth);
378 transferHeader->middleTilesWidth = htons(middleTilesWidth);
379 transferHeader->format0 =
static_cast<unsigned char>(imagePair.
getPixelFormat(0));
380 transferHeader->format1 =
static_cast<unsigned char>(imagePair.
getPixelFormat(1));
381 transferHeader->seqNum =
static_cast<unsigned int>(htonl(imagePair.
getSequenceNumber()));
383 int minDisp = 0, maxDisp = 0;
385 transferHeader->minDisparity = minDisp;
386 transferHeader->maxDisparity = maxDisp;
390 int timeSec = 0, timeMicrosec = 0;
392 transferHeader->timeSec =
static_cast<int>(htonl(static_cast<unsigned int>(timeSec)));
393 transferHeader->timeMicrosec =
static_cast<int>(htonl(static_cast<unsigned int>(timeMicrosec)));
396 memcpy(transferHeader->q, imagePair.
getQMatrix(),
sizeof(float)*16);
400 void ImageProtocol::Pimpl::resetTransfer() {
401 dataProt.resetTransfer();
404 unsigned char* ImageProtocol::Pimpl::getNextReceiveBuffer(
int& maxLength) {
405 maxLength = dataProt.getMaxReceptionSize();
406 return dataProt.getNextReceiveBuffer(maxLength);
409 void ImageProtocol::Pimpl::processReceivedMessage(
int length) {
410 receptionDone =
false;
413 dataProt.processReceivedMessage(length, receptionDone);
415 int receivedBytes = 0;
416 dataProt.getReceivedData(receivedBytes);
419 if(!receiveHeaderParsed) {
421 unsigned char* headerData = dataProt.getReceivedHeader(headerLen);
422 if(headerData !=
nullptr) {
423 tryDecodeHeader(headerData, headerLen);
428 void ImageProtocol::Pimpl::tryDecodeHeader(
const 429 unsigned char* receivedData,
int receivedBytes) {
432 constexpr
int optionalDataSize =
sizeof(receiveHeader.middleTilesWidth);
433 constexpr
int mandatoryDataSize =
static_cast<int>(
sizeof(HeaderData)) - optionalDataSize;
435 if(receivedBytes >= mandatoryDataSize) {
436 receiveHeader = *
reinterpret_cast<const HeaderData*
>(receivedData);
437 if(receiveHeader.magic != htons(MAGIC_SEQUECE)) {
443 if(receiveHeader.protocolVersion != InternalInformation::CURRENT_PROTOCOL_VERSION) {
448 receiveHeader.width = ntohs(receiveHeader.width);
449 receiveHeader.height = ntohs(receiveHeader.height);
450 receiveHeader.firstTileWidth = ntohs(receiveHeader.firstTileWidth);
451 receiveHeader.lastTileWidth = ntohs(receiveHeader.lastTileWidth);
453 receiveHeader.timeSec =
static_cast<int>(
454 htonl(static_cast<unsigned int>(receiveHeader.timeSec)));
455 receiveHeader.timeMicrosec =
static_cast<int>(
456 htonl(static_cast<unsigned int>(receiveHeader.timeMicrosec)));
457 receiveHeader.seqNum = htonl(receiveHeader.seqNum);
460 if(receivedBytes >= mandatoryDataSize + optionalDataSize) {
461 receiveHeader.middleTilesWidth = htons(receiveHeader.middleTilesWidth);
463 receiveHeader.middleTilesWidth = 0;
466 receiveHeaderParsed =
true;
470 bool ImageProtocol::Pimpl::imagesReceived()
const {
471 return receptionDone && receiveHeaderParsed;
474 bool ImageProtocol::Pimpl::getReceivedImagePair(
ImagePair& imagePair) {
475 bool complete =
false;
479 return (ok && complete);
482 bool ImageProtocol::Pimpl::getPartiallyReceivedImagePair(
ImagePair& imagePair,
int& validRows,
bool& complete) {
488 if(!receiveHeaderParsed) {
493 int receivedBytes = 0;
494 unsigned char* data = dataProt.getReceivedData(receivedBytes);
498 imagePair.
setWidth(receiveHeader.width);
499 imagePair.
setHeight(receiveHeader.height);
500 imagePair.
setPixelFormat(0, static_cast<ImagePair::ImageFormat>(receiveHeader.format0));
501 imagePair.
setPixelFormat(1, static_cast<ImagePair::ImageFormat>(receiveHeader.format1));
503 int rowStride0 = 0, rowStride1 = 0;
504 int validRows0 = 0, validRows1 = 0;
505 unsigned char* pixel0 = decodeInterleaved(0, receivedBytes, data, validRows0, rowStride0);
506 unsigned char* pixel1 = decodeInterleaved(1, receivedBytes, data, validRows1, rowStride1);
515 imagePair.
setTimestamp(receiveHeader.timeSec, receiveHeader.timeMicrosec);
516 imagePair.
setDisparityRange(receiveHeader.minDisparity, receiveHeader.maxDisparity);
519 validRows = min(validRows0, validRows1);
521 if(validRows == receiveHeader.height || receptionDone) {
530 unsigned char* ImageProtocol::Pimpl::decodeInterleaved(
int imageNumber,
int receivedBytes,
531 unsigned char* data,
int& validRows,
int& rowStride) {
533 imageNumber == 0 ? receiveHeader.format0 : receiveHeader.format1);
534 int bits0 = getFormatBits(static_cast<ImagePair::ImageFormat>(receiveHeader.format0),
false);
535 int bits1 = getFormatBits(static_cast<ImagePair::ImageFormat>(receiveHeader.format1),
false);
537 unsigned char* ret =
nullptr;
539 if(receiveHeader.lastTileWidth == 0) {
540 int bufferOffset = imageNumber*receiveHeader.width * bits0/8;
541 int bufferRowStride = receiveHeader.width*(bits0 + bits1) / 8;
546 ret = &data[bufferOffset];
547 rowStride = bufferRowStride;
548 validRows = receivedBytes / bufferRowStride;
551 allocateDecodeBuffer(imageNumber);
552 validRows = receivedBytes / bufferRowStride;
553 rowStride = 2*receiveHeader.width;
554 int lastRow = lastReceivedPayloadBytes[imageNumber] / bufferRowStride;
556 BitConversions::decode12BitPacked(lastRow, validRows, &data[bufferOffset],
557 &decodeBuffer[imageNumber][0], bufferRowStride, rowStride, receiveHeader.width);
559 ret = &decodeBuffer[imageNumber][0];
563 decodeTiledImage(imageNumber,
564 lastReceivedPayloadBytes[imageNumber], receivedBytes, data,
565 receiveHeader.firstTileWidth * (bits0 + bits1) / 8,
566 receiveHeader.middleTilesWidth * (bits0 + bits1) / 8,
567 receiveHeader.lastTileWidth * (bits0 + bits1) / 8,
569 ret = &decodeBuffer[imageNumber][0];
570 rowStride = receiveHeader.width*getFormatBits(
571 static_cast<ImagePair::ImageFormat>(format),
true)/8;
574 lastReceivedPayloadBytes[imageNumber] = receivedBytes;
578 void ImageProtocol::Pimpl::allocateDecodeBuffer(
int imageNumber) {
580 imageNumber == 0 ? receiveHeader.format0 : receiveHeader.format1);
581 int bytesPerPixel = getFormatBits(format,
true);
582 int bufferSize = receiveHeader.width * receiveHeader.height * bytesPerPixel;
584 if(decodeBuffer[imageNumber].size() != static_cast<unsigned int>(bufferSize)) {
585 decodeBuffer[imageNumber].resize(bufferSize);
589 void ImageProtocol::Pimpl::decodeTiledImage(
int imageNumber,
int lastReceivedPayloadBytes,
int receivedPayloadBytes,
590 const unsigned char* data,
int firstTileStride,
int middleTilesStride,
int lastTileStride,
int& validRows,
593 allocateDecodeBuffer(imageNumber);
596 int numTiles = getNumTiles(receiveHeader.width, receiveHeader.firstTileWidth,
597 receiveHeader.middleTilesWidth, receiveHeader.lastTileWidth);
598 int payloadOffset = 0;
599 int decodeXOffset = 0;
600 int prevTileStrides = 0;
601 for(
int i = 0; i < numTiles; i++) {
607 tileStride = firstTileStride;
608 tileWidth = receiveHeader.firstTileWidth;
609 }
else if(i == numTiles-1) {
610 tileStride = lastTileStride;
611 tileWidth = receiveHeader.lastTileWidth;
613 tileStride = middleTilesStride;
614 tileWidth = receiveHeader.middleTilesWidth;
617 int tileStart = std::max(0, (lastReceivedPayloadBytes - payloadOffset) / tileStride);
618 int tileStop = std::min(std::max(0, (receivedPayloadBytes - payloadOffset) / tileStride), (
int)receiveHeader.height);
619 int tileOffset = imageNumber * getFormatBits(static_cast<ImagePair::ImageFormat>(receiveHeader.format0),
false) * tileWidth / 8;
621 tileOffset += receiveHeader.height * prevTileStrides;
628 BitConversions::decode12BitPacked(tileStart, tileStop, &data[tileOffset],
629 &decodeBuffer[imageNumber][decodeXOffset], tileStride, 2*receiveHeader.width, tileWidth);
632 decodeRowsFromTile(tileStart, tileStop, &data[tileOffset],
633 &decodeBuffer[imageNumber][decodeXOffset], tileStride,
634 receiveHeader.width*bytesPixel, tileWidth*bytesPixel);
637 payloadOffset += receiveHeader.height * tileStride;
638 decodeXOffset += tileWidth * bytesPixel;
639 prevTileStrides += tileStride;
640 if(i == numTiles-1) {
641 validRows = tileStop;
646 void ImageProtocol::Pimpl::decodeRowsFromTile(
int startRow,
int stopRow,
unsigned const char* src,
647 unsigned char* dst,
int srcStride,
int dstStride,
int tileWidth) {
648 for(
int y = startRow; y < stopRow; y++) {
649 memcpy(&dst[y*dstStride], &src[y*srcStride], tileWidth);
653 void ImageProtocol::Pimpl::resetReception() {
654 receiveHeaderParsed =
false;
655 lastReceivedPayloadBytes[0] = 0;
656 lastReceivedPayloadBytes[1] = 0;
657 dataProt.resetReception(
false);
658 receptionDone =
false;
661 bool ImageProtocol::Pimpl::isConnected()
const {
662 return dataProt.isConnected();
665 const unsigned char* ImageProtocol::Pimpl::getNextControlMessage(
int& length) {
666 return dataProt.getNextControlMessage(length);
669 bool ImageProtocol::Pimpl::newClientConnected() {
670 return dataProt.newClientConnected();
673 int ImageProtocol::Pimpl::getNumDroppedFrames()
const {
674 return dataProt.getDroppedReceptions();
void resetTransfer()
Aborts the transmission of the current transfer and performs a reset of the internal state...
unsigned char * getNextReceiveBuffer(int &maxLength)
Returns the buffer for receiving the next network message.
int getHeight() const
Returns the height of each image.
void setImageDisparityPair(bool dispPair)
Sets whether this is a left camera image and disparity map pair, or two raw camera images...
void processReceivedMessage(int length)
Handles a received network message.
bool getReceivedImagePair(ImagePair &imagePair)
Returns a received image when complete.
bool getPartiallyReceivedImagePair(ImagePair &imagePair, int &validRows, bool &complete)
Returns a partially received image.
void setTransferImagePair(const ImagePair &imagePair)
Sets a new image that will be transfer.
void setPixelFormat(int imageNumber, ImageFormat format)
Sets the pixel format for the given image.
void setSubpixelFactor(int subpixFact)
Sets the subpixel factor for this image pair.
void setPixelData(int imageNumber, unsigned char *pixelData)
Sets the pixel data for the given image.
void setRowStride(int imageNumber, int stride)
Sets a new row stride for the pixel data of one image.
unsigned int getSequenceNumber() const
Returns the sequence number for this image pair.
void setRawTransferData(const ImagePair &metaData, unsigned char *rawData, int firstTileWidth=0, int middleTilesWidth=0, int lastTileWidth=0, int validBytes=0x7FFFFFFF)
Sets the already pre-formatted image data for the next transfer.
void setQMatrix(const float *q)
Sets the pointer to the disparity-to-depth mapping matrix q.
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...
void setDisparityRange(int minimum, int maximum)
Sets the value range for the disparity map contained in this image pair.
bool isConnected() const
Returns true if a remote connection is established.
ImageFormat getPixelFormat(int imageNumber) const
Returns the pixel format for the given image.
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...
int getRowStride(int imageNumber) const
Returns the row stride for the pixel data of one image.
ImageFormat
Image formats that can be transferred.
void setWidth(int w)
Sets a new width for both images.
bool transferComplete()
Returns true if the current transfer has been completed.
void setSequenceNumber(unsigned int num)
Sets the sequence number for this image pair.
ProtocolType
Supported network protocols.
void setTimestamp(int seconds, int microsec)
Sets the time at which this image pair has been captured.
const float * getQMatrix() const
Returns a pointer to the disparity-to-depth mapping matrix q.
bool imagesReceived() const
Returns true if the images of the current transfer have been received.
void setRawValidBytes(int validBytes)
Updates the number of valid bytes in a partial raw transfer.
int getWidth() const
Returns the width of each image.
int getSubpixelFactor() const
Gets the subpixel factor for this image pair.
const unsigned char * getTransferMessage(int &length)
Gets the next network message for the current transfer.
bool isImageDisparityPair() const
Returns true if this is a left camera image and disparity map pair.
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 getTimestamp(int &seconds, int µsec) const
Returns the time at which this image pair has been captured.
Exception class that is used for all protocol exceptions.
unsigned char * getPixelData(int imageNumber) const
Returns the pixel data for the given image.
void setHeight(int h)
Sets a new width for both images.
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.
int getBytesPerPixel(int imageNumber) const
Returns the number of bytes that are required to store one image pixel.