void InputCallback::onRead(yarp::sig::ImageOf<yarp::sig::PixelRgba> &img) #endif { uchar *tmpBuf; QSize s = (QSize(img.width(),img.height())); #if QT_VERSION >= 0x050302 int imgSize = img.getRawImageSize(); #else int imgSize = s.width() * s.height() * img.getPixelSize(); #endif // Allocate a QVideoFrame QVideoFrame frame(imgSize, s, #if QT_VERSION >= 0x050302 img.getRowSize(), #else s.width() * img.getPixelSize(), #endif QVideoFrame::Format_RGB32); // Maps the buffer frame.map(QAbstractVideoBuffer::WriteOnly); // Takes the ownership of the buffer in write only mode tmpBuf = frame.bits(); unsigned char *rawImg = img.getRawImage(); //int j = 0; // Inverts the planes because Qt Wants an image in RGB format instead of BGR /* for(int i=0; i<imgSize; i++){ tmpBuf[j+2] = rawImg[i]; i++; tmpBuf[j+1] = rawImg[i]; i++; tmpBuf[j] = rawImg[i]; tmpBuf[j+3] = 0; j+=4; }*/ #if QT_VERSION >= 0x050302 memcpy(tmpBuf,rawImg,imgSize); #else for(int x =0; x < s.height(); x++) { memcpy(tmpBuf + x * (img.width() * img.getPixelSize()), rawImg + x * (img.getRowSize()), img.width() * img.getPixelSize()); } #endif //unmap the buffer frame.unmap(); if(sigHandler){ sigHandler->sendVideoFrame(frame); } }
bool realsense2Driver::getImage(yarp::sig::ImageOf<yarp::sig::PixelMono>& image) { if (!m_stereoMode) { yError()<<"realsense2Driver: infrared stereo stream not enabled"; return false; } image.resize(width(), height()); std::lock_guard<std::mutex> guard(m_mutex); rs2::frameset data = m_pipeline.wait_for_frames(); rs2::video_frame frm1 = data.get_infrared_frame(1); rs2::video_frame frm2 = data.get_infrared_frame(2); int pixCode = pixFormatToCode(frm1.get_profile().format()); if (pixCode != VOCAB_PIXEL_MONO && pixCode != VOCAB_PIXEL_MONO16) { yError() << "realsense2Driver: expecting Pixel Format MONO or MONO16"; return false; } size_t singleImage_rowSizeByte = image.getRowSize()/2; unsigned char * pixelLeft = (unsigned char*) (frm1.get_data()); unsigned char * pixelRight = (unsigned char*) (frm2.get_data()); unsigned char * pixelOutLeft = image.getRawImage(); unsigned char * pixelOutRight = image.getRawImage() + singleImage_rowSizeByte; for (size_t h=0; h< image.height(); h++) { memcpy(pixelOutLeft, pixelLeft, singleImage_rowSizeByte); memcpy(pixelOutRight, pixelRight, singleImage_rowSizeByte); pixelOutLeft += 2*singleImage_rowSizeByte; pixelOutRight += 2*singleImage_rowSizeByte; pixelLeft += singleImage_rowSizeByte; pixelRight += singleImage_rowSizeByte; } return true; }