예제 #1
0
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);
    }

}
예제 #2
0
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;

}