C++ FlexImage::getRawImage方法代码示例

yarp::os::Things& DepthImageConverter::update(yarp::os::Things& thing)
{
    FlexImage* img = thing.cast_as< FlexImage >();
    inMatrix = (float **) img->getRawImage();

    outImg.setPixelCode(VOCAB_PIXEL_MONO);
    outImg.setPixelSize(1);
    outImg.resize(img->width(), img->height());

    outImg.zero();
    float *inPixels = (float *)img->getRawImage();
    unsigned char *pixels = outImg.getRawImage();
    for(int h=0; h<img->height(); h++)
    {
        for(int w=0; w<img->width(); w++)
        {
            float inVal = inPixels[w + (h * img->width())];
            if (inVal != inVal /* NaN */ || inVal < min || inVal > max) {
                pixels[w + (h * (img->width() ))] = 0;
            } else {
                int val = (int) (255.0 - (inVal * 255.0 / (max - min)));
                if(val >= 255)
                    val = 255;
                if(val <= 0)
                    val = 0;
                pixels[w + (h * (img->width() ))] = (char) val;
            }
        }
    }
    th.setPortWriter(&outImg);
    return th;
}

bool GazeboYarpDepthCameraDriver::getRgbImage(FlexImage& rgbImage, Stamp* timeStamp)
{
    if(!timeStamp)
    {
        myError("timestamp pointer invalid!");
        return false;
    }

    m_colorFrameMutex.wait();

    if(m_width == 0 || m_height == 0)
    {
        myError("gazebo returned an invalid image size");
        m_colorFrameMutex.post();
        return false;
    }
    rgbImage.setPixelCode(m_imageFormat);
    rgbImage.resize(m_width, m_height);
    memcpy(rgbImage.getRawImage(), m_imageFrame_Buffer, m_imageFrame_BufferSize);
#if GAZEBO_MAJOR_VERSION >= 7
    timeStamp->update(this->m_depthCameraSensorPtr->LastUpdateTime().Double());
#else
    timeStamp->update(this->m_depthCameraSensorPtr->GetLastUpdateTime().Double());
#endif

    m_colorFrameMutex.post();
    return true;
}

bool realsense2Driver::getImage(FlexImage& Frame, Stamp *timeStamp, rs2::frameset &sourceFrame)
{
    rs2::video_frame color_frm = sourceFrame.get_color_frame();
    rs2_format format = color_frm.get_profile().format();

    int pixCode = pixFormatToCode(format);
    size_t mem_to_wrt = color_frm.get_width() * color_frm.get_height() * bytesPerPixel(format);

    if (pixCode == VOCAB_PIXEL_INVALID)
    {
        yError() << "realsense2Driver: Pixel Format not recognized";
        return false;
    }

    Frame.setPixelCode(pixCode);
    Frame.resize(m_color_intrin.width, m_color_intrin.height);

    if ((size_t) Frame.getRawImageSize() != mem_to_wrt)
    {
        yError() << "realsense2Driver: device and local copy data size doesn't match";
        return false;
    }

    memcpy((void*)Frame.getRawImage(), (void*)color_frm.get_data(), mem_to_wrt);
    m_rgb_stamp.update();
    *timeStamp = m_rgb_stamp;
    return true;
}

bool MjpegCarrier::write(ConnectionState& proto, SizedWriter& writer) {
    WireImage rep;
    FlexImage *img = rep.checkForImage(writer);

    if (img==NULL) return false;
    int w = img->width();
    int h = img->height();
    int row_stride = img->getRowSize();
    JOCTET *data = (JOCTET*)img->getRawImage();

    JSAMPROW row_pointer[1];

    struct jpeg_compress_struct cinfo;
    struct jpeg_error_mgr jerr;
    cinfo.err = jpeg_std_error(&jerr);
    cinfo.client_data = &proto;
    jpeg_create_compress(&cinfo);
    jpeg_net_dest(&cinfo);
    cinfo.image_width = w;
    cinfo.image_height = h;
    cinfo.input_components = 3;
    cinfo.in_color_space = JCS_RGB;
    jpeg_set_defaults(&cinfo);
    //jpeg_set_quality(&cinfo, 85, TRUE);
    dbg_printf("Starting to compress...\n");
    jpeg_start_compress(&cinfo, TRUE);
    if(!envelope.empty()) {
        jpeg_write_marker(&cinfo, JPEG_COM, reinterpret_cast<const JOCTET*>(envelope.c_str()), envelope.length() + 1);
        envelope.clear();
    }
    dbg_printf("Done compressing (height %d)\n", cinfo.image_height);
    while (cinfo.next_scanline < cinfo.image_height) {
        dbg_printf("Writing row %d...\n", cinfo.next_scanline);
        row_pointer[0] = data + cinfo.next_scanline * row_stride;
        jpeg_write_scanlines(&cinfo, row_pointer, 1);
    }
    jpeg_finish_compress(&cinfo);
    jpeg_destroy_compress(&cinfo);

    return true;
}

bool Image::read(yarp::os::ConnectionReader& connection) {

    // auto-convert text mode interaction
    connection.convertTextMode();
    
    ImageNetworkHeader header;
    
    bool ok = connection.expectBlock((char*)&header,sizeof(header));
    if (!ok) return false;
        
    imgPixelCode = header.id;
    
    int q = getQuantum();
    if (q==0) {
        //q = YARP_IMAGE_ALIGN;
        setQuantum(header.quantum);
        q = getQuantum();
    }
    if (q!=header.quantum) {
        if ((header.depth*header.width)%header.quantum==0 &&
            (header.depth*header.width)%q==0) {
            header.quantum = q;
        }
    }
        
    if (getPixelCode()!=header.id||q!=header.quantum) {
        // we're trying to read an incompatible image type
        // rather than just fail, we'll read it (inefficiently)
        FlexImage flex;
        flex.setPixelCode(header.id);
        flex.setQuantum(header.quantum);
        flex.resize(header.width,header.height);
        if (header.width!=0&&header.height!=0) {
            unsigned char *mem = flex.getRawImage();
            yAssert(mem!=NULL);
            if (flex.getRawImageSize()!=header.imgSize) {
                printf("There is a problem reading an image\n");
                printf("incoming: width %d, height %d, code %d, quantum %d, size %d\n",
                       (int)header.width, (int)header.height, 
                       (int)header.id,
                       (int)header.quantum, (int)header.imgSize);
                printf("my space: width %d, height %d, code %d, quantum %d, size %d\n",
                       flex.width(), flex.height(), flex.getPixelCode(),
                       flex.getQuantum(), 
                       flex.getRawImageSize());
            }
            yAssert(flex.getRawImageSize()==header.imgSize);
            ok = connection.expectBlock((char *)flex.getRawImage(),
                                        flex.getRawImageSize());
            if (!ok) return false;
        }
        copy(flex);
    } else {
        yAssert(getPixelCode()==header.id);
        resize(header.width,header.height);
        unsigned char *mem = getRawImage();
        if (header.width!=0&&header.height!=0) {
            yAssert(mem!=NULL);
            if (getRawImageSize()!=header.imgSize) {
                printf("There is a problem reading an image\n");
                printf("incoming: width %d, height %d, code %d, quantum %d, size %d\n",
                       (int)header.width, (int)header.height, 
                       (int)header.id,
                       (int)header.quantum, (int)header.imgSize);
                printf("my space: width %d, height %d, code %d, quantum %d, size %d\n",
                       width(), height(), getPixelCode(), getQuantum(), getRawImageSize());
            }
            yAssert(getRawImageSize()==header.imgSize);
            ok = connection.expectBlock((char *)getRawImage(),
                                        getRawImageSize());
            if (!ok) return false;
        }
    }
    
    return !connection.isError();
}