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 secondTileWidth = 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 secondTileWidth;
90 unsigned char format0;
91 unsigned char format1;
92 unsigned short minDisparity;
93 unsigned short maxDisparity;
94 unsigned char subpixelFactor;
106 ProtocolType protType;
109 std::vector<unsigned char> headerBuffer;
110 std::vector<unsigned char> rawBuffer;
111 unsigned char* rawData;
114 std::vector<unsigned char, AlignedAllocator<unsigned char> >decodeBuffer[2];
115 bool receiveHeaderParsed;
116 HeaderData receiveHeader;
117 int lastReceivedPayloadBytes[2];
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,
136 void decodeTiledImage(
int imageNumber,
int lastReceivedPayloadBytes,
int receivedPayloadBytes,
137 const unsigned char* data,
int firstTileStride,
int secondTileStride,
int& validRows,
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);
149 ImageProtocol::ImageProtocol(
bool server,
ProtocolType protType,
int maxUdpPacketSize)
150 : pimpl(new Pimpl(server, 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 pimpl->processReceivedMessage(length);
205 return pimpl->getNumDroppedFrames();
209 pimpl->resetReception();
213 return pimpl->isConnected();
217 return pimpl->getNextControlMessage(length);
221 return pimpl->newClientConnected();
226 ImageProtocol::Pimpl::Pimpl(
bool server,
ProtocolType protType,
int maxUdpPacketSize)
227 :dataProt(server, (DataBlockProtocol::ProtocolType)protType,
228 maxUdpPacketSize), protType(protType), rawData(
nullptr),
229 receiveHeaderParsed(
false), lastReceivedPayloadBytes{0, 0},
230 receptionDone(
false) {
231 headerBuffer.resize(
sizeof(HeaderData) + 32);
232 memset(&headerBuffer[0], 0,
sizeof(headerBuffer.size()));
233 memset(&receiveHeader, 0,
sizeof(receiveHeader));
236 void ImageProtocol::Pimpl::setTransferImagePair(
const ImagePair& imagePair) {
242 int rawDataLength = getFrameSize(imagePair.
getWidth(), imagePair.
getHeight(), 0, 0,
246 copyHeaderToBuffer(imagePair, 0, 0, &headerBuffer[16]);
247 dataProt.resetTransfer();
248 dataProt.setTransferHeader(&headerBuffer[16],
sizeof(HeaderData), rawDataLength);
251 int bits[2] = {0, 0};
252 int rowSize[2] = {0, 0};
253 int rowStride[2] = {0, 0};
254 const unsigned char* pixelData[2] = {
nullptr,
nullptr};
255 std::vector<unsigned char> encodingBuffer[2];
257 for(
int i = 0; i<2; i++) {
259 rowSize[i] = imagePair.
getWidth()*bits[i]/8;
265 encodingBuffer[i].resize(rowSize[i] * imagePair.
getHeight());
268 pixelData[i] = &encodingBuffer[i][0];
269 rowStride[i] = rowSize[i];
274 rawBuffer.resize(imagePair.
getWidth()*imagePair.
getHeight()*(bits[0] + bits[1])/8 +
sizeof(
int));
275 int bufferOffset = 0;
277 for(
int y = 0; y<imagePair.
getHeight(); y++) {
278 memcpy(&rawBuffer[bufferOffset], &pixelData[0][y*rowStride[0]], rowSize[0]);
279 bufferOffset += rowSize[0];
281 memcpy(&rawBuffer[bufferOffset], &pixelData[1][y*rowStride[1]], rowSize[1]);
282 bufferOffset += rowSize[1];
285 rawData = &rawBuffer[0];
286 int rawValidBytes =
static_cast<int>(rawBuffer.size() -
sizeof(int));
288 dataProt.setTransferData(rawData, rawValidBytes);
291 void ImageProtocol::Pimpl::setRawTransferData(
const ImagePair& metaData,
unsigned char* rawData,
292 int firstTileWidth,
int secondTileWidth,
int validBytes) {
293 if(rawData ==
nullptr) {
303 copyHeaderToBuffer(metaData, firstTileWidth, secondTileWidth, &headerBuffer[16]);
304 dataProt.resetTransfer();
305 dataProt.setTransferHeader(&headerBuffer[16],
sizeof(HeaderData), rawDataLength);
307 this->rawData = rawData;
309 dataProt.setTransferData(rawData, validBytes);
312 void ImageProtocol::Pimpl::setRawValidBytes(
int validBytes) {
313 dataProt.setTransferValidBytes(validBytes);
316 const unsigned char* ImageProtocol::Pimpl::getTransferMessage(
int& length) {
317 const unsigned char* msg = dataProt.getTransferMessage(length);
320 msg = dataProt.getTransferMessage(length);
326 bool ImageProtocol::Pimpl::transferComplete() {
327 return dataProt.transferComplete();
330 int ImageProtocol::Pimpl::getFrameSize(
int width,
int height,
int firstTileWidth,
334 int bits0 = getFormatBits(format0,
false);
335 int bits1 = getFormatBits(format1,
false);
337 int effectiveWidth = firstTileWidth > 0 ? firstTileWidth + secondTileWidth : width;
339 return (effectiveWidth * height * (bits0 + bits1)) /8;
355 void ImageProtocol::Pimpl::copyHeaderToBuffer(
const ImagePair& imagePair,
356 int firstTileWidth,
int secondTileWidth,
unsigned char* buffer) {
357 HeaderData* transferHeader =
reinterpret_cast<HeaderData*
>(buffer);
358 memset(transferHeader, 0,
sizeof(*transferHeader));
359 transferHeader->magic = htons(MAGIC_SEQUECE);
360 transferHeader->protocolVersion = InternalInformation::CURRENT_PROTOCOL_VERSION;
362 transferHeader->width = htons(imagePair.
getWidth());
363 transferHeader->height = htons(imagePair.
getHeight());
364 transferHeader->firstTileWidth = htons(firstTileWidth);
365 transferHeader->secondTileWidth = htons(secondTileWidth);
366 transferHeader->format0 =
static_cast<unsigned char>(imagePair.
getPixelFormat(0));
367 transferHeader->format1 =
static_cast<unsigned char>(imagePair.
getPixelFormat(1));
368 transferHeader->seqNum =
static_cast<unsigned int>(htonl(imagePair.
getSequenceNumber()));
370 int minDisp = 0, maxDisp = 0;
372 transferHeader->minDisparity = minDisp;
373 transferHeader->maxDisparity = maxDisp;
377 int timeSec = 0, timeMicrosec = 0;
379 transferHeader->timeSec =
static_cast<int>(htonl(static_cast<unsigned int>(timeSec)));
380 transferHeader->timeMicrosec =
static_cast<int>(htonl(static_cast<unsigned int>(timeMicrosec)));
383 memcpy(transferHeader->q, imagePair.
getQMatrix(),
sizeof(float)*16);
387 void ImageProtocol::Pimpl::resetTransfer() {
388 dataProt.resetTransfer();
391 unsigned char* ImageProtocol::Pimpl::getNextReceiveBuffer(
int& maxLength) {
392 maxLength = dataProt.getMaxReceptionSize();
393 return dataProt.getNextReceiveBuffer(maxLength);
396 void ImageProtocol::Pimpl::processReceivedMessage(
int length) {
397 receptionDone =
false;
400 dataProt.processReceivedMessage(length, receptionDone);
402 int receivedBytes = 0;
403 dataProt.getReceivedData(receivedBytes);
406 if(!receiveHeaderParsed) {
408 unsigned char* headerData = dataProt.getReceivedHeader(headerLen);
409 if(headerData !=
nullptr) {
410 tryDecodeHeader(headerData, headerLen);
415 void ImageProtocol::Pimpl::tryDecodeHeader(
const 416 unsigned char* receivedData,
int receivedBytes) {
417 if(receivedBytes >= static_cast<int>(
sizeof(HeaderData))) {
418 receiveHeader = *
reinterpret_cast<const HeaderData*
>(receivedData);
419 if(receiveHeader.magic != htons(MAGIC_SEQUECE)) {
425 if(receiveHeader.protocolVersion > InternalInformation::CURRENT_PROTOCOL_VERSION ||
426 receiveHeader.protocolVersion < 4) {
431 receiveHeader.width = ntohs(receiveHeader.width);
432 receiveHeader.height = ntohs(receiveHeader.height);
433 receiveHeader.firstTileWidth = ntohs(receiveHeader.firstTileWidth);
434 receiveHeader.secondTileWidth = ntohs(receiveHeader.secondTileWidth);
435 receiveHeader.timeSec =
static_cast<int>(
436 htonl(static_cast<unsigned int>(receiveHeader.timeSec)));
437 receiveHeader.timeMicrosec =
static_cast<int>(
438 htonl(static_cast<unsigned int>(receiveHeader.timeMicrosec)));
439 receiveHeader.seqNum = htonl(receiveHeader.seqNum);
441 receiveHeaderParsed =
true;
445 bool ImageProtocol::Pimpl::imagesReceived()
const {
446 return receptionDone && receiveHeaderParsed;
449 bool ImageProtocol::Pimpl::getReceivedImagePair(
ImagePair& imagePair) {
450 bool complete =
false;
454 return (ok && complete);
457 bool ImageProtocol::Pimpl::getPartiallyReceivedImagePair(
ImagePair& imagePair,
int& validRows,
bool& complete) {
463 if(!receiveHeaderParsed) {
468 int receivedBytes = 0;
469 unsigned char* data = dataProt.getReceivedData(receivedBytes);
473 imagePair.
setWidth(receiveHeader.width);
474 imagePair.
setHeight(receiveHeader.height);
475 imagePair.
setPixelFormat(0, static_cast<ImagePair::ImageFormat>(receiveHeader.format0));
476 imagePair.
setPixelFormat(1, static_cast<ImagePair::ImageFormat>(receiveHeader.format1));
478 int rowStride0 = 0, rowStride1 = 0;
479 int validRows0 = 0, validRows1 = 0;
480 unsigned char* pixel0 = decodeInterleaved(0, receivedBytes, data, validRows0, rowStride0);
481 unsigned char* pixel1 = decodeInterleaved(1, receivedBytes, data, validRows1, rowStride1);
490 imagePair.
setTimestamp(receiveHeader.timeSec, receiveHeader.timeMicrosec);
491 imagePair.
setDisparityRange(receiveHeader.minDisparity, receiveHeader.maxDisparity);
494 validRows = min(validRows0, validRows1);
496 if(validRows == receiveHeader.height || receptionDone) {
505 unsigned char* ImageProtocol::Pimpl::decodeInterleaved(
int imageNumber,
int receivedBytes,
506 unsigned char* data,
int& validRows,
int& rowStride) {
508 imageNumber == 0 ? receiveHeader.format0 : receiveHeader.format1);
509 int bits0 = getFormatBits(static_cast<ImagePair::ImageFormat>(receiveHeader.format0),
false);
510 int bits1 = getFormatBits(static_cast<ImagePair::ImageFormat>(receiveHeader.format1),
false);
512 unsigned char* ret =
nullptr;
514 if(receiveHeader.secondTileWidth == 0) {
515 int bufferOffset = imageNumber*receiveHeader.width * bits0/8;
516 int bufferRowStride = receiveHeader.width*(bits0 + bits1) / 8;
521 ret = &data[bufferOffset];
522 rowStride = bufferRowStride;
523 validRows = receivedBytes / bufferRowStride;
526 allocateDecodeBuffer(imageNumber);
527 validRows = receivedBytes / bufferRowStride;
528 rowStride = 2*receiveHeader.width;
529 int lastRow = lastReceivedPayloadBytes[imageNumber] / bufferRowStride;
531 BitConversions::decode12BitPacked(lastRow, validRows, &data[bufferOffset],
532 &decodeBuffer[imageNumber][0], bufferRowStride, rowStride, receiveHeader.width);
534 ret = &decodeBuffer[imageNumber][0];
538 decodeTiledImage(imageNumber,
539 lastReceivedPayloadBytes[imageNumber], receivedBytes,
540 data, receiveHeader.firstTileWidth * (bits0 + bits1) / 8,
541 receiveHeader.secondTileWidth * (bits0 + bits1) / 8,
543 ret = &decodeBuffer[imageNumber][0];
544 rowStride = receiveHeader.width*getFormatBits(
545 static_cast<ImagePair::ImageFormat>(format),
true)/8;
548 lastReceivedPayloadBytes[imageNumber] = receivedBytes;
552 void ImageProtocol::Pimpl::allocateDecodeBuffer(
int imageNumber) {
554 imageNumber == 0 ? receiveHeader.format0 : receiveHeader.format1);
555 int bytesPerPixel = getFormatBits(format,
true);
556 int bufferSize = receiveHeader.width * receiveHeader.height * bytesPerPixel;
558 if(decodeBuffer[imageNumber].size() != static_cast<unsigned int>(bufferSize)) {
559 decodeBuffer[imageNumber].resize(bufferSize);
563 void ImageProtocol::Pimpl::decodeTiledImage(
int imageNumber,
int lastReceivedPayloadBytes,
int receivedPayloadBytes,
564 const unsigned char* data,
int firstTileStride,
int secondTileStride,
int& validRows,
568 allocateDecodeBuffer(imageNumber);
571 int startFirstTile = lastReceivedPayloadBytes / firstTileStride;
572 int stopFirstTile = std::min(receivedPayloadBytes / firstTileStride,
573 static_cast<int>(receiveHeader.height));
576 int secondTileBytes = receivedPayloadBytes - (receiveHeader.height*firstTileStride);
577 int lastSecondTileBytes = lastReceivedPayloadBytes - (receiveHeader.height*firstTileStride);
578 int startSecondTile = std::max(0, lastSecondTileBytes / secondTileStride);
579 int stopSecondTile = std::max(0, secondTileBytes / secondTileStride);
580 int firstTileOffset = imageNumber * getFormatBits(
581 static_cast<ImagePair::ImageFormat>(receiveHeader.format0),
false) * receiveHeader.firstTileWidth / 8;
587 BitConversions::decode12BitPacked(startFirstTile, stopFirstTile, &data[firstTileOffset], &decodeBuffer[imageNumber][0],
588 firstTileStride, 2*receiveHeader.width, receiveHeader.firstTileWidth);
591 decodeRowsFromTile(startFirstTile, stopFirstTile, &data[firstTileOffset],
592 &decodeBuffer[imageNumber][0], firstTileStride, receiveHeader.width*bytesPixel,
593 receiveHeader.firstTileWidth*bytesPixel);
597 int secondTileOffset = receiveHeader.height*firstTileStride +
598 imageNumber * getFormatBits(static_cast<ImagePair::ImageFormat>(receiveHeader.format0),
false) *
599 receiveHeader.secondTileWidth / 8;
602 BitConversions::decode12BitPacked(startSecondTile, stopSecondTile,
603 &data[secondTileOffset], &decodeBuffer[imageNumber][2*receiveHeader.firstTileWidth],
604 secondTileStride, 2*receiveHeader.width, receiveHeader.secondTileWidth);
606 decodeRowsFromTile(startSecondTile, stopSecondTile, &data[secondTileOffset],
607 &decodeBuffer[imageNumber][receiveHeader.firstTileWidth*bytesPixel],
608 secondTileStride, receiveHeader.width*bytesPixel, receiveHeader.secondTileWidth*bytesPixel);
611 validRows = stopSecondTile;
614 void ImageProtocol::Pimpl::decodeRowsFromTile(
int startRow,
int stopRow,
unsigned const char* src,
615 unsigned char* dst,
int srcStride,
int dstStride,
int tileWidth) {
616 for(
int y = startRow; y < stopRow; y++) {
617 memcpy(&dst[y*dstStride], &src[y*srcStride], tileWidth);
621 void ImageProtocol::Pimpl::resetReception() {
622 receiveHeaderParsed =
false;
623 lastReceivedPayloadBytes[0] = 0;
624 lastReceivedPayloadBytes[1] = 0;
625 dataProt.resetReception(
false);
626 receptionDone =
false;
629 bool ImageProtocol::Pimpl::isConnected()
const {
630 return dataProt.isConnected();
633 const unsigned char* ImageProtocol::Pimpl::getNextControlMessage(
int& length) {
634 return dataProt.getNextControlMessage(length);
637 bool ImageProtocol::Pimpl::newClientConnected() {
638 return dataProt.newClientConnected();
641 int ImageProtocol::Pimpl::getNumDroppedFrames()
const {
642 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 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.
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 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.