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

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 decompress(const Bytes& cimg, FlexImage& img) {
        bool debug = false;

        if (!active) {
            init();
            active = true;
        }
        cinfo.client_data = &error_buffer;
        cinfo.err = jpeg_std_error(&jerr.pub);
        jerr.pub.error_exit = net_error_exit;

        if (setjmp(jerr.setjmp_buffer)) {
            jpeg_finish_decompress(&cinfo);
            return false;
        }

        jpeg_net_src(&cinfo,(char*)cimg.get(),cimg.length());
        jpeg_save_markers(&cinfo, JPEG_COM, 0xFFFF);
        jpeg_read_header(&cinfo, TRUE);
        jpeg_calc_output_dimensions(&cinfo);

        if(cinfo.jpeg_color_space == JCS_GRAYSCALE) {
            img.setPixelCode(VOCAB_PIXEL_MONO);
        }
        else
        {
            img.setPixelCode(VOCAB_PIXEL_RGB);
        }

        if (debug) printf("Got image %dx%d\n", cinfo.output_width, cinfo.output_height);
        img.resize(cinfo.output_width,cinfo.output_height);
        jpeg_start_decompress(&cinfo);
        //int row_stride = cinfo.output_width * cinfo.output_components;

        int at = 0;
        while (cinfo.output_scanline < cinfo.output_height) {
            JSAMPLE *lines[1];
            lines[0] = (JSAMPLE*)(img.getPixelAddress(0,at));
            jpeg_read_scanlines(&cinfo, lines, 1);
            at++;
        }
        if(readEnvelopeCallback && cinfo.marker_list && cinfo.marker_list->data_length > 0) {
            Bytes envelope(reinterpret_cast<char*>(cinfo.marker_list->data), cinfo.marker_list->data_length);
            readEnvelopeCallback(readEnvelopeCallbackData, envelope);
        }
        if (debug) printf("Read image!\n");
        jpeg_finish_decompress(&cinfo);
        return true;
    }

 void testCreate() {
     report(0,"testing image creation...");
     FlexImage image;
     image.setPixelCode(VOCAB_PIXEL_RGB);
     image.resize(256,128);
     checkEqual(image.width(),256,"check width");
     checkEqual(image.height(),128,"check height");
     ImageOf<PixelInt> iint;
     iint.resize(256,128);
     long int total = 0;
     for (int x=0; x<iint.width(); x++) {
         for (int y=0; y<iint.height(); y++) {
             int v = (x+y)%65537;
             iint.pixel(x,y) = v;
             total += v;
         }
     }
     for (int x2=0; x2<iint.width(); x2++) {
         for (int y2=0; y2<iint.height(); y2++) {
             total -= iint.pixel(x2,y2);
         }
     }
     checkEqual(total,0,"pixel assignment check");
 }

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();
}