C++ FlexImage类代码示例

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

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 FlexLoadBMP8u(const char *file, FlexImage<Im8u,1> &img) 
{
    FILE *in;
	BITMAPFILEHDR bmfh;
	BITMAPINFOHDR bmih;	

	in=fopen(file,"rb");
	if (in == NULL)
		return(false);
        //WARNING: Extra padding in bmfh causes erroneous reading into all but first field of structure
	fread(&bmfh,BFHSIZE,1,in);							//read BMP header
        ConvertBmfh(&bmfh);
	if (bmfh.bfType != 19778)							//check for valid BMP file
		return(false);
	fread(&bmih,BIHSIZE,1,in);							//read info header
        ConvertBmih(&bmih);
	if (bmih.biBitCount == 8)	
	{	fclose(in);										//load as grayscale
		return FlexLoadBMPGray(file, img);
	}
	if (bmih.biBitCount == 24)
	{	fclose(in);
		FlexImage<Im8u,3> tmp;
		bool ok = FlexLoadBMPColor(file, tmp);			//load as color image
		if (ok)
		{	img.ReallocateNE(tmp.Width(), tmp.Height());
			FlexRGBToGray(tmp, img, false);					//convert to grayscale
		}
		return ok;
	}
	fclose(in);
	return(false);										//only read 8 bit images
}

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

    void testExternal() {
        report(0, "testing external image...");
        unsigned char buf[EXT_HEIGHT][EXT_WIDTH];

        {
            for (int x=0; x<EXT_WIDTH; x++) {
                for (int y=0; y<EXT_HEIGHT; y++) {
                    buf[y][x] = 20;
                }
            }
        }

        ImageOf<PixelMono> img1;

        img1.setExternal(&buf[0][0],EXT_WIDTH,EXT_HEIGHT);

        checkEqual(img1.width(),EXT_WIDTH,"width check");
        checkEqual(img1.height(),EXT_HEIGHT,"height check");

        int mismatch = 0;
        for (int x=0; x<img1.width(); x++) {
            for (int y=0; y<img1.height(); y++) {
                img1.pixel(x,y) = 5;
                if (buf[y][x]!=5) {
                    mismatch++;
                }
            }
        }
        checkEqual(mismatch,0,"delta check");

        report(0, "testing various padding + alignments...");
        for (int ww=1; ww<=17; ww++) {
            for (int hh=1; hh<=17; hh++) {
                for (int pad1=1; pad1<=9; pad1++) {
                    for (int pad2=1; pad2<=9; pad2++) {
                        int wwp1 = (ww%pad1)?(ww+pad1-(ww%pad1)):ww;
                        FlexImage img;
                        char *data = new char[wwp1*hh*3];
                        yAssert(data);
                        img.setQuantum(pad1);
                        img.setPixelCode(VOCAB_PIXEL_RGB);
                        img.setPixelSize(3);
                        img.setExternal(data,ww,hh);
                        ImageOf<PixelRgb> target;
                        target.setQuantum(pad2);
                        target.copy(img);
                        delete[] data;
                    }
                }
            }
        }
    }

    bool getVideo(ImageOf<PixelRgb>& image) {
        if (frameFinished) {
            FlexImage flex;
            flex.setPixelCode(VOCAB_PIXEL_RGB);
            flex.setQuantum((pFrameRGB->linesize[0]));
            flex.setExternal(pFrameRGB->data[0],
                             pCodecCtx->width,
                             pCodecCtx->height);
            image.copy(flex);
        }

        return frameFinished;
    }

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

bool DepthImageConverter::accept(yarp::os::Things& thing)
{
    FlexImage* img = thing.cast_as< FlexImage >();
    if(img == NULL) {
        printf("DepthImageConverter: expected type FlexImage but got wrong data type!\n");
        return false;
    }

    if( img->getPixelCode() == VOCAB_PIXEL_MONO_FLOAT)
    {
        return true;
    }

    printf("DepthImageConverter: expected %s, got %s, not doing any convertion!\n", yarp::os::Vocab::decode(VOCAB_PIXEL_MONO_FLOAT).c_str(), yarp::os::Vocab::decode(img->getPixelCode()).c_str() );
    return false;
}

bool FlexLoadBMPColor(const char *file, FlexImage<Im8u,3> &img) 
{
    FILE *in;
	BITMAPFILEHDR bmfh;
	BITMAPINFOHDR bmih;	

	in=fopen(file,"rb");
	if (in == NULL)
		return(false);	
        //WARNING: Extra padding in bmfh causes erroneous reading into all but first field of structure
	fread(&bmfh,BFHSIZE,1,in);							//read BMP header
        ConvertBmfh(&bmfh);
	if (bmfh.bfType != 19778)							//check for valid BMP file
		return(false);
	fread(&bmih,BIHSIZE,1,in);							//read info header
        ConvertBmih(&bmih);
	if (bmih.biBitCount != 24) 
		return(false);									//only read 8 bit images
	img.Reallocate(bmih.biWidth, abs(bmih.biHeight));	//allocate image to size

	RGBA tmp;
	for(int i = 0; i < (int)bmih.biClrUsed; i++)		//skip color info
		fread(&tmp, sizeof(RGBA), 1, in);

	int padWidth = 3 * bmih.biWidth;
	while(padWidth%4) padWidth++;						
	int dir = 1, yv = 0;
	if (bmih.biHeight > 0)
	{	dir = -1;
		yv = img.Height() - 1;
	}
	for(int y = 0; y < img.Height(); y++ )				//read in pixel data
	{	char tmp;
		fread(&img(0,yv), 1, img.Width() * 3, in);
		yv += dir;
		for(int i = 0; i < padWidth - img.Width() * 3; i++)	//skip over pad bytes
			fread(&tmp, 1, 1, in);
	}
	fclose(in);
	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 FlexFilterRowVOMP(FlexImage<T,1> &src, FlexImage<T,1> &dst, T2 *kernel, int kernelSize, bool allocate = true)
{
	if(kernelSize % 2 == 0)									//enforce odd length kernels
		return false;
	if(allocate)
		dst.Reallocate(src.Size());
	dst.Set((T)0);
	int n = kernelSize / 2, h = src.Height();
	#pragma omp parallel for
	for(int y = 0; y < h; y++)
	{	T *psrc = &src(n, y), *pdst = &dst(n, y);
		for(int x = n; x < src.Width() - n; x++)
		{	int k = 0;
			T2 acc = 0;
			for(int i = -n; i <= n; i++) 
				acc += (T2)(psrc[i] * kernel[k++]);
			*pdst = (T)acc;
			pdst++;
			psrc++;
		}
	}
	return true;
}

bool LogPolarTransformThread::threadInit() 
{
    /* grab an image to set the image size */
    FlexImage *image;
    do {
        image = imagePortIn->read(true);
    } while (image == NULL && !isStopping());

    if (isStopping())
        return false;

    const int width  = image->width();
    const int height = image->height();
    // the logpolar mapping has always depth 3 (RGB) but we need to copy the input image in case it's monochrome.
    const int depth = 3; 
 
    cout << "||| logPolarTransformThread: width = " << *xSizeValue << " height = " << *ySizeValue << endl;
    cout << "||| logPolarTransformThread: angles = " << *anglesValue << " rings = " << *ringsValue << endl;

    /* create the input image of the correct resolution  */
    if (*directionValue == CARTESIAN2LOGPOLAR) {
        *xSizeValue = width;
        *ySizeValue = height;
    }

    cout << "||| initializing the logpolar mapping" << endl;
    if (!allocLookupTables(*directionValue, *ringsValue, *anglesValue, *xSizeValue, *ySizeValue, *overlapValue)) {
        cerr << "can't allocate lookup tables" << endl;
        return false;
    }
    cout << "||| lookup table allocation done" << endl;

    inputImage = new ImageOf<PixelRgb>;
    inputImage->resize(width, height);

    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 FlexFilterColumnVOMP(FlexImage<T,1> &src, FlexImage<T,1> &dst, T2 *kernel, int kernelSize, bool allocate = true)
{
	if(kernelSize % 2 == 0)									//enforce odd length kernels
		return false;
	if(allocate)
		dst.Reallocate(src.Size());
	dst.Set((T)0);
	int n = kernelSize / 2;
	int sb = src.StepBytes(), h = src.Height() - n;
	#pragma omp parallel for
	for(int y = n; y < h; y++)
	{	T *psrc = &src(0, y), *pdst = &dst(0, y);
		for(int x = 0; x < src.Width(); x++)
		{	int k = 0;
			T2 acc = 0;
			for(int i = -n; i <= n; i++) 
				acc += (T2)(*(T *)((char *)psrc + sb * i) * kernel[k++]);
			*pdst = (T)acc;
			pdst++;
			psrc++;
		}
	}
	return true;
}

bool Image::copy(const Image& alt, int w, int h) {
    if (getPixelCode()==0) {
        setPixelCode(alt.getPixelCode());
        setPixelSize(alt.getPixelSize());
        setQuantum(alt.getQuantum());
    }
    if (&alt==this) {
        FlexImage img;
        img.copy(alt);
        return copy(img,w,h);
    }

    if (getPixelCode()!=alt.getPixelCode()) {
        FlexImage img;
        img.setPixelCode(getPixelCode());
        img.setPixelSize(getPixelSize());
        img.setQuantum(getQuantum());
        img.copy(alt);
        return copy(img,w,h);
    }

    resize(w,h);
    int d = getPixelSize();

    int nw = w;
    int nh = h;
    w = alt.width();
    h = alt.height();

    float di = ((float)h)/nh;
    float dj = ((float)w)/nw;

    for (int i=0; i<nh; i++)
        {
            int i0 = (int)(di*i);
            for (int j=0; j<nw; j++)
                {
                    int j0 = (int)(dj*j);
                    memcpy(getPixelAddress(j,i),
                           alt.getPixelAddress(j0,i0),
                           d);
                }
        }
    return true;
}