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> 42 class ImageProtocol::Pimpl {
44 Pimpl(
bool server, ProtocolType protType,
int maxUdpPacketSize);
47 void setTransferImagePair(
const ImagePair& imagePair);
48 void setRawTransferData(
const ImagePair& metaData,
unsigned char* rawData,
49 int firstTileWidth = 0,
int secondTileWidth = 0,
int validBytes = 0x7FFFFFFF);
50 void setRawValidBytes(
int validBytes);
51 const unsigned char* getTransferMessage(
int& length);
52 bool transferComplete();
54 bool getReceivedImagePair(
ImagePair& imagePair);
55 bool getPartiallyReceivedImagePair(
ImagePair& imagePair,
56 int& validRows,
bool& complete);
57 bool imagesReceived()
const;
59 unsigned char* getNextReceiveBuffer(
int& maxLength);
61 void processReceivedMessage(
int length);
62 int getProspectiveMessageSize();
63 int getNumDroppedFrames()
const;
64 void resetReception();
65 bool isConnected()
const;
66 const unsigned char* getNextControlMessage(
int& length);
67 bool newClientConnected();
70 unsigned short MAGIC_SEQUECE = 0x3D15;
77 unsigned char protocolVersion;
78 unsigned char isRawImagePair;
81 unsigned short height;
83 unsigned short firstTileWidth;
84 unsigned short secondTileWidth;
86 unsigned char format0;
87 unsigned char format1;
88 unsigned char minDisparity;
89 unsigned char maxDisparity;
101 ProtocolType protType;
104 std::vector<unsigned char> headerBuffer;
105 std::vector<unsigned char> rawBuffer;
106 unsigned char* rawData;
109 std::vector<unsigned char, AlignedAllocator<unsigned char> >decodeBuffer[2];
110 bool receiveHeaderParsed;
111 HeaderData receiveHeader;
112 int lastReceivedPayloadBytes[2];
116 void copyHeaderToBuffer(
const ImagePair& imagePair,
int firstTileWidth,
117 int secondTileWidth,
unsigned char* buffer);
120 void tryDecodeHeader(
const unsigned char* receivedData,
int receivedBytes);
123 unsigned char* decodeInterleaved(
int imageNumber,
int receivedBytes,
124 unsigned char* data,
int& validRows,
int& rowStride);
126 int getFrameSize(
int width,
int height,
int firstTileWidth,
int secondTileWidth,
131 void decodeTiledImage(
int imageNumber,
int lastReceivedPayloadBytes,
int receivedPayloadBytes,
132 const unsigned char* data,
int firstTileStride,
int secondTileStride,
int& validRows,
135 void decodeRowsFromTile(
int startRow,
int stopRow,
unsigned const char* src,
136 unsigned char* dst,
int srcStride,
int dstStride,
int tileWidth);
138 void allocateDecodeBuffer(
int imageNumber);
145 : pimpl(new Pimpl(server, protType, maxUdpPacketSize)) {
149 ImageProtocol::~ImageProtocol() {
154 pimpl->setTransferImagePair(imagePair);
158 unsigned char* imageData,
int firstTileWidth,
int secondTileWidth,
int validBytes) {
159 pimpl->setRawTransferData(metaData, imageData, firstTileWidth, secondTileWidth, validBytes);
163 pimpl->setRawValidBytes(validBytes);
167 return pimpl->getTransferMessage(length);
171 return pimpl->transferComplete();
175 pimpl->resetTransfer();
179 return pimpl->getReceivedImagePair(imagePair);
183 ImagePair& imagePair,
int& validRows,
bool& complete) {
184 return pimpl->getPartiallyReceivedImagePair(imagePair, validRows, complete);
188 return pimpl->imagesReceived();
192 return pimpl->getNextReceiveBuffer(maxLength);
196 pimpl->processReceivedMessage(length);
200 return pimpl->getNumDroppedFrames();
204 pimpl->resetReception();
208 return pimpl->isConnected();
212 return pimpl->getNextControlMessage(length);
216 return pimpl->newClientConnected();
221 ImageProtocol::Pimpl::Pimpl(
bool server,
ProtocolType protType,
int maxUdpPacketSize)
222 :dataProt(server, (DataBlockProtocol::ProtocolType)protType,
223 maxUdpPacketSize), protType(protType), rawData(
nullptr),
224 receiveHeaderParsed(
false), lastReceivedPayloadBytes{0, 0},
225 receptionDone(
false) {
226 headerBuffer.resize(
sizeof(HeaderData) + 32);
227 memset(&headerBuffer[0], 0,
sizeof(headerBuffer.size()));
228 memset(&receiveHeader, 0,
sizeof(receiveHeader));
231 void ImageProtocol::Pimpl::setTransferImagePair(
const ImagePair& imagePair) {
237 int rawDataLength = getFrameSize(imagePair.
getWidth(), imagePair.
getHeight(), 0, 0,
241 copyHeaderToBuffer(imagePair, 0, 0, &headerBuffer[16]);
242 dataProt.resetTransfer();
243 dataProt.setTransferHeader(&headerBuffer[16],
sizeof(HeaderData), rawDataLength);
246 int bits[2] = {0, 0};
247 int rowSize[2] = {0, 0};
248 int rowStride[2] = {0, 0};
249 const unsigned char* pixelData[2] = {
nullptr,
nullptr};
250 std::vector<unsigned char> encodingBuffer[2];
252 for(
int i = 0; i<2; i++) {
254 rowSize[i] = imagePair.
getWidth()*bits[i]/8;
260 encodingBuffer[i].resize(rowSize[i] * imagePair.
getHeight());
263 pixelData[i] = &encodingBuffer[i][0];
264 rowStride[i] = rowSize[i];
269 rawBuffer.resize(imagePair.
getWidth()*imagePair.
getHeight()*(bits[0] + bits[1])/8 +
sizeof(
int));
270 int bufferOffset = 0;
272 for(
int y = 0; y<imagePair.
getHeight(); y++) {
273 memcpy(&rawBuffer[bufferOffset], &pixelData[0][y*rowStride[0]], rowSize[0]);
274 bufferOffset += rowSize[0];
276 memcpy(&rawBuffer[bufferOffset], &pixelData[1][y*rowStride[1]], rowSize[1]);
277 bufferOffset += rowSize[1];
280 rawData = &rawBuffer[0];
281 int rawValidBytes =
static_cast<int>(rawBuffer.size() -
sizeof(int));
283 dataProt.setTransferData(rawData, rawValidBytes);
286 void ImageProtocol::Pimpl::setRawTransferData(
const ImagePair& metaData,
unsigned char* rawData,
287 int firstTileWidth,
int secondTileWidth,
int validBytes) {
288 if(rawData ==
nullptr) {
298 copyHeaderToBuffer(metaData, firstTileWidth, secondTileWidth, &headerBuffer[16]);
299 dataProt.resetTransfer();
300 dataProt.setTransferHeader(&headerBuffer[16],
sizeof(HeaderData), rawDataLength);
302 this->rawData = rawData;
304 dataProt.setTransferData(rawData, validBytes);
307 void ImageProtocol::Pimpl::setRawValidBytes(
int validBytes) {
308 dataProt.setTransferValidBytes(validBytes);
311 const unsigned char* ImageProtocol::Pimpl::getTransferMessage(
int& length) {
312 const unsigned char* msg = dataProt.getTransferMessage(length);
315 msg = dataProt.getTransferMessage(length);
321 bool ImageProtocol::Pimpl::transferComplete() {
322 return dataProt.transferComplete();
325 int ImageProtocol::Pimpl::getFrameSize(
int width,
int height,
int firstTileWidth,
329 int bits0 = getFormatBits(format0,
false);
330 int bits1 = getFormatBits(format1,
false);
332 int effectiveWidth = firstTileWidth > 0 ? firstTileWidth + secondTileWidth : width;
334 return (effectiveWidth * height * (bits0 + bits1)) /8;
350 void ImageProtocol::Pimpl::copyHeaderToBuffer(
const ImagePair& imagePair,
351 int firstTileWidth,
int secondTileWidth,
unsigned char* buffer) {
352 HeaderData* transferHeader =
reinterpret_cast<HeaderData*
>(buffer);
353 memset(transferHeader, 0,
sizeof(*transferHeader));
354 transferHeader->magic = htons(MAGIC_SEQUECE);
355 transferHeader->protocolVersion = InternalInformation::CURRENT_PROTOCOL_VERSION;
357 transferHeader->width = htons(imagePair.
getWidth());
358 transferHeader->height = htons(imagePair.
getHeight());
359 transferHeader->firstTileWidth = htons(firstTileWidth);
360 transferHeader->secondTileWidth = htons(secondTileWidth);
361 transferHeader->format0 =
static_cast<unsigned char>(imagePair.
getPixelFormat(0));
362 transferHeader->format1 =
static_cast<unsigned char>(imagePair.
getPixelFormat(1));
363 transferHeader->seqNum =
static_cast<unsigned int>(htonl(imagePair.
getSequenceNumber()));
365 int minDisp = 0, maxDisp = 0;
367 transferHeader->minDisparity = minDisp;
368 transferHeader->maxDisparity = maxDisp;
370 int timeSec = 0, timeMicrosec = 0;
372 transferHeader->timeSec =
static_cast<int>(htonl(static_cast<unsigned int>(timeSec)));
373 transferHeader->timeMicrosec =
static_cast<int>(htonl(static_cast<unsigned int>(timeMicrosec)));
376 memcpy(transferHeader->q, imagePair.
getQMatrix(),
sizeof(float)*16);
380 void ImageProtocol::Pimpl::resetTransfer() {
381 dataProt.resetTransfer();
384 unsigned char* ImageProtocol::Pimpl::getNextReceiveBuffer(
int& maxLength) {
385 maxLength = dataProt.getMaxReceptionSize();
386 return dataProt.getNextReceiveBuffer(maxLength);
389 void ImageProtocol::Pimpl::processReceivedMessage(
int length) {
390 receptionDone =
false;
393 dataProt.processReceivedMessage(length, receptionDone);
395 int receivedBytes = 0;
396 dataProt.getReceivedData(receivedBytes);
399 if(!receiveHeaderParsed) {
401 unsigned char* headerData = dataProt.getReceivedHeader(headerLen);
402 if(headerData !=
nullptr) {
403 tryDecodeHeader(headerData, headerLen);
408 void ImageProtocol::Pimpl::tryDecodeHeader(
const 409 unsigned char* receivedData,
int receivedBytes) {
410 if(receivedBytes >= static_cast<int>(
sizeof(HeaderData))) {
411 receiveHeader = *
reinterpret_cast<const HeaderData*
>(receivedData);
412 if(receiveHeader.magic != htons(MAGIC_SEQUECE)) {
418 if(receiveHeader.protocolVersion > InternalInformation::CURRENT_PROTOCOL_VERSION ||
419 receiveHeader.protocolVersion < 4) {
424 receiveHeader.width = ntohs(receiveHeader.width);
425 receiveHeader.height = ntohs(receiveHeader.height);
426 receiveHeader.firstTileWidth = ntohs(receiveHeader.firstTileWidth);
427 receiveHeader.secondTileWidth = ntohs(receiveHeader.secondTileWidth);
428 receiveHeader.timeSec =
static_cast<int>(
429 htonl(static_cast<unsigned int>(receiveHeader.timeSec)));
430 receiveHeader.timeMicrosec =
static_cast<int>(
431 htonl(static_cast<unsigned int>(receiveHeader.timeMicrosec)));
432 receiveHeader.seqNum = htonl(receiveHeader.seqNum);
434 receiveHeaderParsed =
true;
438 bool ImageProtocol::Pimpl::imagesReceived()
const {
439 return receptionDone && receiveHeaderParsed;
442 bool ImageProtocol::Pimpl::getReceivedImagePair(
ImagePair& imagePair) {
443 bool complete =
false;
447 return (ok && complete);
450 bool ImageProtocol::Pimpl::getPartiallyReceivedImagePair(
ImagePair& imagePair,
int& validRows,
bool& complete) {
456 if(!receiveHeaderParsed) {
461 int receivedBytes = 0;
462 unsigned char* data = dataProt.getReceivedData(receivedBytes);
466 imagePair.
setWidth(receiveHeader.width);
467 imagePair.
setHeight(receiveHeader.height);
468 imagePair.
setPixelFormat(0, static_cast<ImagePair::ImageFormat>(receiveHeader.format0));
469 imagePair.
setPixelFormat(1, static_cast<ImagePair::ImageFormat>(receiveHeader.format1));
471 int rowStride0 = 0, rowStride1 = 0;
472 int validRows0 = 0, validRows1 = 0;
473 unsigned char* pixel0 = decodeInterleaved(0, receivedBytes, data, validRows0, rowStride0);
474 unsigned char* pixel1 = decodeInterleaved(1, receivedBytes, data, validRows1, rowStride1);
483 imagePair.
setTimestamp(receiveHeader.timeSec, receiveHeader.timeMicrosec);
484 imagePair.
setDisparityRange(receiveHeader.minDisparity, receiveHeader.maxDisparity);
486 validRows = min(validRows0, validRows1);
488 if(validRows == receiveHeader.height || receptionDone) {
497 unsigned char* ImageProtocol::Pimpl::decodeInterleaved(
int imageNumber,
int receivedBytes,
498 unsigned char* data,
int& validRows,
int& rowStride) {
500 imageNumber == 0 ? receiveHeader.format0 : receiveHeader.format1);
501 int bits0 = getFormatBits(static_cast<ImagePair::ImageFormat>(receiveHeader.format0),
false);
502 int bits1 = getFormatBits(static_cast<ImagePair::ImageFormat>(receiveHeader.format1),
false);
504 unsigned char* ret =
nullptr;
506 if(receiveHeader.secondTileWidth == 0) {
507 int bufferOffset = imageNumber*receiveHeader.width * bits0/8;
508 int bufferRowStride = receiveHeader.width*(bits0 + bits1) / 8;
513 ret = &data[bufferOffset];
514 rowStride = bufferRowStride;
515 validRows = receivedBytes / bufferRowStride;
518 allocateDecodeBuffer(imageNumber);
519 validRows = receivedBytes / bufferRowStride;
520 rowStride = 2*receiveHeader.width;
521 int lastRow = lastReceivedPayloadBytes[imageNumber] / bufferRowStride;
523 BitConversions::decode12BitPacked(lastRow, validRows, &data[bufferOffset],
524 &decodeBuffer[imageNumber][0], bufferRowStride, rowStride, receiveHeader.width);
526 ret = &decodeBuffer[imageNumber][0];
530 decodeTiledImage(imageNumber,
531 lastReceivedPayloadBytes[imageNumber], receivedBytes,
532 data, receiveHeader.firstTileWidth * (bits0 + bits1) / 8,
533 receiveHeader.secondTileWidth * (bits0 + bits1) / 8,
535 ret = &decodeBuffer[imageNumber][0];
536 rowStride = receiveHeader.width*getFormatBits(
537 static_cast<ImagePair::ImageFormat>(format),
true)/8;
540 lastReceivedPayloadBytes[imageNumber] = receivedBytes;
544 void ImageProtocol::Pimpl::allocateDecodeBuffer(
int imageNumber) {
546 imageNumber == 0 ? receiveHeader.format0 : receiveHeader.format1);
547 int bytesPerPixel = getFormatBits(format,
true);
548 int bufferSize = receiveHeader.width * receiveHeader.height * bytesPerPixel;
550 if(decodeBuffer[imageNumber].size() != static_cast<unsigned int>(bufferSize)) {
551 decodeBuffer[imageNumber].resize(bufferSize);
555 void ImageProtocol::Pimpl::decodeTiledImage(
int imageNumber,
int lastReceivedPayloadBytes,
int receivedPayloadBytes,
556 const unsigned char* data,
int firstTileStride,
int secondTileStride,
int& validRows,
560 allocateDecodeBuffer(imageNumber);
563 int startFirstTile = lastReceivedPayloadBytes / firstTileStride;
564 int stopFirstTile = std::min(receivedPayloadBytes / firstTileStride,
565 static_cast<int>(receiveHeader.height));
568 int secondTileBytes = receivedPayloadBytes - (receiveHeader.height*firstTileStride);
569 int lastSecondTileBytes = lastReceivedPayloadBytes - (receiveHeader.height*firstTileStride);
570 int startSecondTile = std::max(0, lastSecondTileBytes / secondTileStride);
571 int stopSecondTile = std::max(0, secondTileBytes / secondTileStride);
572 int firstTileOffset = imageNumber * getFormatBits(
573 static_cast<ImagePair::ImageFormat>(receiveHeader.format0),
false) * receiveHeader.firstTileWidth / 8;
577 BitConversions::decode12BitPacked(startFirstTile, stopFirstTile, &data[firstTileOffset], &decodeBuffer[imageNumber][0],
578 firstTileStride, 2*receiveHeader.width, receiveHeader.firstTileWidth);
580 decodeRowsFromTile(startFirstTile, stopFirstTile, &data[firstTileOffset],
581 &decodeBuffer[imageNumber][0], firstTileStride, receiveHeader.width,
582 receiveHeader.firstTileWidth);
586 int secondTileOffset = receiveHeader.height*firstTileStride +
587 imageNumber * getFormatBits(static_cast<ImagePair::ImageFormat>(receiveHeader.format0),
false) * receiveHeader.secondTileWidth / 8;
590 BitConversions::decode12BitPacked(startSecondTile, stopSecondTile,
591 &data[secondTileOffset], &decodeBuffer[imageNumber][2*receiveHeader.firstTileWidth],
592 secondTileStride, 2*receiveHeader.width, receiveHeader.secondTileWidth);
594 decodeRowsFromTile(startSecondTile, stopSecondTile, &data[secondTileOffset],
595 &decodeBuffer[imageNumber][receiveHeader.firstTileWidth],
596 secondTileStride, receiveHeader.width, receiveHeader.secondTileWidth);
599 validRows = stopSecondTile;
602 void ImageProtocol::Pimpl::decodeRowsFromTile(
int startRow,
int stopRow,
unsigned const char* src,
603 unsigned char* dst,
int srcStride,
int dstStride,
int tileWidth) {
604 for(
int y = startRow; y < stopRow; y++) {
605 memcpy(&dst[y*dstStride], &src[y*srcStride], tileWidth);
609 void ImageProtocol::Pimpl::resetReception() {
610 receiveHeaderParsed =
false;
611 lastReceivedPayloadBytes[0] = 0;
612 lastReceivedPayloadBytes[1] = 0;
613 dataProt.resetReception(
false);
614 receptionDone =
false;
617 bool ImageProtocol::Pimpl::isConnected()
const {
618 return dataProt.isConnected();
621 const unsigned char* ImageProtocol::Pimpl::getNextControlMessage(
int& length) {
622 return dataProt.getNextControlMessage(length);
625 bool ImageProtocol::Pimpl::newClientConnected() {
626 return dataProt.newClientConnected();
629 int ImageProtocol::Pimpl::getNumDroppedFrames()
const {
630 return dataProt.getDroppedReceptions();
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...
int getNumDroppedFrames() const
Returns the number of frames that have been dropped since connecting to the current remote host...
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.
ImageProtocol(bool server, ProtocolType protType, int maxUdpPacketSize=1472)
Creates a new instance for decoding / encoding network messages for the given network protocol...
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.
const float * getQMatrix() const
Returns a pointer to the disparity-to-depth mapping matrix q.
int getWidth() const
Returns the width of each image.
bool isConnected() const
Returns true if a remote connection is established.
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.
int getBytesPerPixel(int imageNumber) const
Returns the number of bytes that are required to store one image pixel.
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.
void setTransferImagePair(const ImagePair &imagePair)
Sets a new image that will be transfer.
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.
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 processReceivedMessage(int length)
Handles a received network message.
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.