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> 44 class ImageProtocol::Pimpl {
46 Pimpl(
bool server, ProtocolType protType,
int maxUdpPacketSize);
49 void setTransferImagePair(
const ImagePair& imagePair);
50 void setRawTransferData(
const ImagePair& metaData,
unsigned char* rawData,
51 int firstTileWidth = 0,
int secondTileWidth = 0,
int validBytes = 0x7FFFFFFF);
52 void setRawValidBytes(
int validBytes);
53 const unsigned char* getTransferMessage(
int& length);
54 bool transferComplete();
56 bool getReceivedImagePair(
ImagePair& imagePair);
57 bool getPartiallyReceivedImagePair(
ImagePair& imagePair,
58 int& validRows,
bool& complete);
59 bool imagesReceived()
const;
61 unsigned char* getNextReceiveBuffer(
int& maxLength);
63 void processReceivedMessage(
int length);
64 int getProspectiveMessageSize();
65 int getNumDroppedFrames()
const;
66 void resetReception();
67 bool isConnected()
const;
68 const unsigned char* getNextControlMessage(
int& length);
69 bool newClientConnected();
72 unsigned short MAGIC_SEQUECE = 0x3D15;
79 unsigned char protocolVersion;
80 unsigned char isRawImagePair;
83 unsigned short height;
85 unsigned short firstTileWidth;
86 unsigned short secondTileWidth;
88 unsigned char format0;
89 unsigned char format1;
90 unsigned char minDisparity;
91 unsigned char maxDisparity;
103 ProtocolType protType;
106 std::vector<unsigned char> headerBuffer;
107 std::vector<unsigned char> rawBuffer;
108 unsigned char* rawData;
111 std::vector<unsigned char, AlignedAllocator<unsigned char> >decodeBuffer[2];
112 bool receiveHeaderParsed;
113 HeaderData receiveHeader;
114 int lastReceivedPayloadBytes[2];
118 void copyHeaderToBuffer(
const ImagePair& imagePair,
int firstTileWidth,
119 int secondTileWidth,
unsigned char* buffer);
122 void tryDecodeHeader(
const unsigned char* receivedData,
int receivedBytes);
125 unsigned char* decodeInterleaved(
int imageNumber,
int receivedBytes,
126 unsigned char* data,
int& validRows,
int& rowStride);
128 int getFrameSize(
int width,
int height,
int firstTileWidth,
int secondTileWidth,
133 void decodeTiledImage(
int imageNumber,
int lastReceivedPayloadBytes,
int receivedPayloadBytes,
134 const unsigned char* data,
int firstTileStride,
int secondTileStride,
int& validRows,
137 void decodeRowsFromTile(
int startRow,
int stopRow,
unsigned const char* src,
138 unsigned char* dst,
int srcStride,
int dstStride,
int tileWidth);
140 void allocateDecodeBuffer(
int imageNumber);
146 ImageProtocol::ImageProtocol(
bool server,
ProtocolType protType,
int maxUdpPacketSize)
147 : pimpl(new Pimpl(server, protType, maxUdpPacketSize)) {
151 ImageProtocol::~ImageProtocol() {
156 pimpl->setTransferImagePair(imagePair);
160 unsigned char* imageData,
int firstTileWidth,
int secondTileWidth,
int validBytes) {
161 pimpl->setRawTransferData(metaData, imageData, firstTileWidth, secondTileWidth, validBytes);
165 pimpl->setRawValidBytes(validBytes);
169 return pimpl->getTransferMessage(length);
173 return pimpl->transferComplete();
177 pimpl->resetTransfer();
181 return pimpl->getReceivedImagePair(imagePair);
185 ImagePair& imagePair,
int& validRows,
bool& complete) {
186 return pimpl->getPartiallyReceivedImagePair(imagePair, validRows, complete);
190 return pimpl->imagesReceived();
194 return pimpl->getNextReceiveBuffer(maxLength);
198 pimpl->processReceivedMessage(length);
202 return pimpl->getNumDroppedFrames();
206 pimpl->resetReception();
210 return pimpl->isConnected();
214 return pimpl->getNextControlMessage(length);
218 return pimpl->newClientConnected();
223 ImageProtocol::Pimpl::Pimpl(
bool server,
ProtocolType protType,
int maxUdpPacketSize)
224 :dataProt(server, (DataBlockProtocol::ProtocolType)protType,
225 maxUdpPacketSize), protType(protType), rawData(
nullptr),
226 receiveHeaderParsed(
false), lastReceivedPayloadBytes{0, 0},
227 receptionDone(
false) {
228 headerBuffer.resize(
sizeof(HeaderData) + 32);
229 memset(&headerBuffer[0], 0,
sizeof(headerBuffer.size()));
230 memset(&receiveHeader, 0,
sizeof(receiveHeader));
233 void ImageProtocol::Pimpl::setTransferImagePair(
const ImagePair& imagePair) {
239 int rawDataLength = getFrameSize(imagePair.
getWidth(), imagePair.
getHeight(), 0, 0,
243 copyHeaderToBuffer(imagePair, 0, 0, &headerBuffer[16]);
244 dataProt.resetTransfer();
245 dataProt.setTransferHeader(&headerBuffer[16],
sizeof(HeaderData), rawDataLength);
248 int bits[2] = {0, 0};
249 int rowSize[2] = {0, 0};
250 int rowStride[2] = {0, 0};
251 const unsigned char* pixelData[2] = {
nullptr,
nullptr};
252 std::vector<unsigned char> encodingBuffer[2];
254 for(
int i = 0; i<2; i++) {
256 rowSize[i] = imagePair.
getWidth()*bits[i]/8;
262 encodingBuffer[i].resize(rowSize[i] * imagePair.
getHeight());
265 pixelData[i] = &encodingBuffer[i][0];
266 rowStride[i] = rowSize[i];
271 rawBuffer.resize(imagePair.
getWidth()*imagePair.
getHeight()*(bits[0] + bits[1])/8 +
sizeof(
int));
272 int bufferOffset = 0;
274 for(
int y = 0; y<imagePair.
getHeight(); y++) {
275 memcpy(&rawBuffer[bufferOffset], &pixelData[0][y*rowStride[0]], rowSize[0]);
276 bufferOffset += rowSize[0];
278 memcpy(&rawBuffer[bufferOffset], &pixelData[1][y*rowStride[1]], rowSize[1]);
279 bufferOffset += rowSize[1];
282 rawData = &rawBuffer[0];
283 int rawValidBytes =
static_cast<int>(rawBuffer.size() -
sizeof(int));
285 dataProt.setTransferData(rawData, rawValidBytes);
288 void ImageProtocol::Pimpl::setRawTransferData(
const ImagePair& metaData,
unsigned char* rawData,
289 int firstTileWidth,
int secondTileWidth,
int validBytes) {
290 if(rawData ==
nullptr) {
300 copyHeaderToBuffer(metaData, firstTileWidth, secondTileWidth, &headerBuffer[16]);
301 dataProt.resetTransfer();
302 dataProt.setTransferHeader(&headerBuffer[16],
sizeof(HeaderData), rawDataLength);
304 this->rawData = rawData;
306 dataProt.setTransferData(rawData, validBytes);
309 void ImageProtocol::Pimpl::setRawValidBytes(
int validBytes) {
310 dataProt.setTransferValidBytes(validBytes);
313 const unsigned char* ImageProtocol::Pimpl::getTransferMessage(
int& length) {
314 const unsigned char* msg = dataProt.getTransferMessage(length);
317 msg = dataProt.getTransferMessage(length);
323 bool ImageProtocol::Pimpl::transferComplete() {
324 return dataProt.transferComplete();
327 int ImageProtocol::Pimpl::getFrameSize(
int width,
int height,
int firstTileWidth,
331 int bits0 = getFormatBits(format0,
false);
332 int bits1 = getFormatBits(format1,
false);
334 int effectiveWidth = firstTileWidth > 0 ? firstTileWidth + secondTileWidth : width;
336 return (effectiveWidth * height * (bits0 + bits1)) /8;
352 void ImageProtocol::Pimpl::copyHeaderToBuffer(
const ImagePair& imagePair,
353 int firstTileWidth,
int secondTileWidth,
unsigned char* buffer) {
354 HeaderData* transferHeader =
reinterpret_cast<HeaderData*
>(buffer);
355 memset(transferHeader, 0,
sizeof(*transferHeader));
356 transferHeader->magic = htons(MAGIC_SEQUECE);
357 transferHeader->protocolVersion = InternalInformation::CURRENT_PROTOCOL_VERSION;
359 transferHeader->width = htons(imagePair.
getWidth());
360 transferHeader->height = htons(imagePair.
getHeight());
361 transferHeader->firstTileWidth = htons(firstTileWidth);
362 transferHeader->secondTileWidth = htons(secondTileWidth);
363 transferHeader->format0 =
static_cast<unsigned char>(imagePair.
getPixelFormat(0));
364 transferHeader->format1 =
static_cast<unsigned char>(imagePair.
getPixelFormat(1));
365 transferHeader->seqNum =
static_cast<unsigned int>(htonl(imagePair.
getSequenceNumber()));
367 int minDisp = 0, maxDisp = 0;
369 transferHeader->minDisparity = minDisp;
370 transferHeader->maxDisparity = maxDisp;
372 int timeSec = 0, timeMicrosec = 0;
374 transferHeader->timeSec =
static_cast<int>(htonl(static_cast<unsigned int>(timeSec)));
375 transferHeader->timeMicrosec =
static_cast<int>(htonl(static_cast<unsigned int>(timeMicrosec)));
378 memcpy(transferHeader->q, imagePair.
getQMatrix(),
sizeof(float)*16);
382 void ImageProtocol::Pimpl::resetTransfer() {
383 dataProt.resetTransfer();
386 unsigned char* ImageProtocol::Pimpl::getNextReceiveBuffer(
int& maxLength) {
387 maxLength = dataProt.getMaxReceptionSize();
388 return dataProt.getNextReceiveBuffer(maxLength);
391 void ImageProtocol::Pimpl::processReceivedMessage(
int length) {
392 receptionDone =
false;
395 dataProt.processReceivedMessage(length, receptionDone);
397 int receivedBytes = 0;
398 dataProt.getReceivedData(receivedBytes);
401 if(!receiveHeaderParsed) {
403 unsigned char* headerData = dataProt.getReceivedHeader(headerLen);
404 if(headerData !=
nullptr) {
405 tryDecodeHeader(headerData, headerLen);
410 void ImageProtocol::Pimpl::tryDecodeHeader(
const 411 unsigned char* receivedData,
int receivedBytes) {
412 if(receivedBytes >= static_cast<int>(
sizeof(HeaderData))) {
413 receiveHeader = *
reinterpret_cast<const HeaderData*
>(receivedData);
414 if(receiveHeader.magic != htons(MAGIC_SEQUECE)) {
420 if(receiveHeader.protocolVersion > InternalInformation::CURRENT_PROTOCOL_VERSION ||
421 receiveHeader.protocolVersion < 4) {
426 receiveHeader.width = ntohs(receiveHeader.width);
427 receiveHeader.height = ntohs(receiveHeader.height);
428 receiveHeader.firstTileWidth = ntohs(receiveHeader.firstTileWidth);
429 receiveHeader.secondTileWidth = ntohs(receiveHeader.secondTileWidth);
430 receiveHeader.timeSec =
static_cast<int>(
431 htonl(static_cast<unsigned int>(receiveHeader.timeSec)));
432 receiveHeader.timeMicrosec =
static_cast<int>(
433 htonl(static_cast<unsigned int>(receiveHeader.timeMicrosec)));
434 receiveHeader.seqNum = htonl(receiveHeader.seqNum);
436 receiveHeaderParsed =
true;
440 bool ImageProtocol::Pimpl::imagesReceived()
const {
441 return receptionDone && receiveHeaderParsed;
444 bool ImageProtocol::Pimpl::getReceivedImagePair(
ImagePair& imagePair) {
445 bool complete =
false;
449 return (ok && complete);
452 bool ImageProtocol::Pimpl::getPartiallyReceivedImagePair(
ImagePair& imagePair,
int& validRows,
bool& complete) {
458 if(!receiveHeaderParsed) {
463 int receivedBytes = 0;
464 unsigned char* data = dataProt.getReceivedData(receivedBytes);
468 imagePair.
setWidth(receiveHeader.width);
469 imagePair.
setHeight(receiveHeader.height);
470 imagePair.
setPixelFormat(0, static_cast<ImagePair::ImageFormat>(receiveHeader.format0));
471 imagePair.
setPixelFormat(1, static_cast<ImagePair::ImageFormat>(receiveHeader.format1));
473 int rowStride0 = 0, rowStride1 = 0;
474 int validRows0 = 0, validRows1 = 0;
475 unsigned char* pixel0 = decodeInterleaved(0, receivedBytes, data, validRows0, rowStride0);
476 unsigned char* pixel1 = decodeInterleaved(1, receivedBytes, data, validRows1, rowStride1);
485 imagePair.
setTimestamp(receiveHeader.timeSec, receiveHeader.timeMicrosec);
486 imagePair.
setDisparityRange(receiveHeader.minDisparity, receiveHeader.maxDisparity);
488 validRows = min(validRows0, validRows1);
490 if(validRows == receiveHeader.height || receptionDone) {
499 unsigned char* ImageProtocol::Pimpl::decodeInterleaved(
int imageNumber,
int receivedBytes,
500 unsigned char* data,
int& validRows,
int& rowStride) {
502 imageNumber == 0 ? receiveHeader.format0 : receiveHeader.format1);
503 int bits0 = getFormatBits(static_cast<ImagePair::ImageFormat>(receiveHeader.format0),
false);
504 int bits1 = getFormatBits(static_cast<ImagePair::ImageFormat>(receiveHeader.format1),
false);
506 unsigned char* ret =
nullptr;
508 if(receiveHeader.secondTileWidth == 0) {
509 int bufferOffset = imageNumber*receiveHeader.width * bits0/8;
510 int bufferRowStride = receiveHeader.width*(bits0 + bits1) / 8;
515 ret = &data[bufferOffset];
516 rowStride = bufferRowStride;
517 validRows = receivedBytes / bufferRowStride;
520 allocateDecodeBuffer(imageNumber);
521 validRows = receivedBytes / bufferRowStride;
522 rowStride = 2*receiveHeader.width;
523 int lastRow = lastReceivedPayloadBytes[imageNumber] / bufferRowStride;
525 BitConversions::decode12BitPacked(lastRow, validRows, &data[bufferOffset],
526 &decodeBuffer[imageNumber][0], bufferRowStride, rowStride, receiveHeader.width);
528 ret = &decodeBuffer[imageNumber][0];
532 decodeTiledImage(imageNumber,
533 lastReceivedPayloadBytes[imageNumber], receivedBytes,
534 data, receiveHeader.firstTileWidth * (bits0 + bits1) / 8,
535 receiveHeader.secondTileWidth * (bits0 + bits1) / 8,
537 ret = &decodeBuffer[imageNumber][0];
538 rowStride = receiveHeader.width*getFormatBits(
539 static_cast<ImagePair::ImageFormat>(format),
true)/8;
542 lastReceivedPayloadBytes[imageNumber] = receivedBytes;
546 void ImageProtocol::Pimpl::allocateDecodeBuffer(
int imageNumber) {
548 imageNumber == 0 ? receiveHeader.format0 : receiveHeader.format1);
549 int bytesPerPixel = getFormatBits(format,
true);
550 int bufferSize = receiveHeader.width * receiveHeader.height * bytesPerPixel;
552 if(decodeBuffer[imageNumber].size() != static_cast<unsigned int>(bufferSize)) {
553 decodeBuffer[imageNumber].resize(bufferSize);
557 void ImageProtocol::Pimpl::decodeTiledImage(
int imageNumber,
int lastReceivedPayloadBytes,
int receivedPayloadBytes,
558 const unsigned char* data,
int firstTileStride,
int secondTileStride,
int& validRows,
562 allocateDecodeBuffer(imageNumber);
565 int startFirstTile = lastReceivedPayloadBytes / firstTileStride;
566 int stopFirstTile = std::min(receivedPayloadBytes / firstTileStride,
567 static_cast<int>(receiveHeader.height));
570 int secondTileBytes = receivedPayloadBytes - (receiveHeader.height*firstTileStride);
571 int lastSecondTileBytes = lastReceivedPayloadBytes - (receiveHeader.height*firstTileStride);
572 int startSecondTile = std::max(0, lastSecondTileBytes / secondTileStride);
573 int stopSecondTile = std::max(0, secondTileBytes / secondTileStride);
574 int firstTileOffset = imageNumber * getFormatBits(
575 static_cast<ImagePair::ImageFormat>(receiveHeader.format0),
false) * receiveHeader.firstTileWidth / 8;
581 BitConversions::decode12BitPacked(startFirstTile, stopFirstTile, &data[firstTileOffset], &decodeBuffer[imageNumber][0],
582 firstTileStride, 2*receiveHeader.width, receiveHeader.firstTileWidth);
585 decodeRowsFromTile(startFirstTile, stopFirstTile, &data[firstTileOffset],
586 &decodeBuffer[imageNumber][0], firstTileStride, receiveHeader.width*bytesPixel,
587 receiveHeader.firstTileWidth*bytesPixel);
591 int secondTileOffset = receiveHeader.height*firstTileStride +
592 imageNumber * getFormatBits(static_cast<ImagePair::ImageFormat>(receiveHeader.format0),
false) *
593 receiveHeader.secondTileWidth / 8;
596 BitConversions::decode12BitPacked(startSecondTile, stopSecondTile,
597 &data[secondTileOffset], &decodeBuffer[imageNumber][2*receiveHeader.firstTileWidth],
598 secondTileStride, 2*receiveHeader.width, receiveHeader.secondTileWidth);
600 decodeRowsFromTile(startSecondTile, stopSecondTile, &data[secondTileOffset],
601 &decodeBuffer[imageNumber][receiveHeader.firstTileWidth*bytesPixel],
602 secondTileStride, receiveHeader.width*bytesPixel, receiveHeader.secondTileWidth*bytesPixel);
605 validRows = stopSecondTile;
608 void ImageProtocol::Pimpl::decodeRowsFromTile(
int startRow,
int stopRow,
unsigned const char* src,
609 unsigned char* dst,
int srcStride,
int dstStride,
int tileWidth) {
610 for(
int y = startRow; y < stopRow; y++) {
611 memcpy(&dst[y*dstStride], &src[y*srcStride], tileWidth);
615 void ImageProtocol::Pimpl::resetReception() {
616 receiveHeaderParsed =
false;
617 lastReceivedPayloadBytes[0] = 0;
618 lastReceivedPayloadBytes[1] = 0;
619 dataProt.resetReception(
false);
620 receptionDone =
false;
623 bool ImageProtocol::Pimpl::isConnected()
const {
624 return dataProt.isConnected();
627 const unsigned char* ImageProtocol::Pimpl::getNextControlMessage(
int& length) {
628 return dataProt.getNextControlMessage(length);
631 bool ImageProtocol::Pimpl::newClientConnected() {
632 return dataProt.newClientConnected();
635 int ImageProtocol::Pimpl::getNumDroppedFrames()
const {
636 return dataProt.getDroppedReceptions();
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...
int getNumDroppedFrames() const
Returns the number of frames that have been dropped since connecting to the current remote host...
void setPixelFormat(int imageNumber, ImageFormat format)
Sets the pixel format for the given image.
bool transferComplete()
Returns true if the current transfer has been completed.
void setRawValidBytes(int validBytes)
Updates the number of valid bytes in a partial raw transfer.
bool imagesReceived() const
Returns true if the images of the current transfer have been received.
bool isConnected() const
Returns true if a remote connection is established.
void processReceivedMessage(int length)
Handles a received network message.
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 secondTileWidth=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.
bool newClientConnected()
Returns true if the last message has established a new connection from a client.
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 getPartiallyReceivedImagePair(ImagePair &imagePair, int &validRows, bool &complete)
Returns a partially received 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...
ImageFormat getPixelFormat(int imageNumber) const
Returns the pixel format for the given image.
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.
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.
bool getReceivedImagePair(ImagePair &imagePair)
Returns a received image when complete.
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 resetReception()
Aborts the reception of the current image transfer and resets the internal state. ...
void setTransferImagePair(const ImagePair &imagePair)
Sets a new image that will be 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.
void resetTransfer()
Aborts the transmission of the current transfer and performs a reset of the internal state...
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.
A set of two images, which are usually the left camera image and the disparity map.
const unsigned char * getTransferMessage(int &length)
Gets the next network message for the current transfer.
unsigned char * getNextReceiveBuffer(int &maxLength)
Returns the buffer for receiving the next network message.
int getBytesPerPixel(int imageNumber) const
Returns the number of bytes that are required to store one image pixel.