Beispiel #1
0
void RGBDSensorWrapper::shallowCopyImages(const ImageOf<PixelFloat>& src, ImageOf<PixelFloat>& dest)
{
    dest.setQuantum(src.getQuantum());
    dest.setExternal(src.getRawImage(), src.width(), src.height());
}
bool ImageSplitter::updateModule()
{
    ImageOf<PixelRgb> *inputImage    = inputPort.read();
    yarp::os::Stamp stamp;
    inputPort.getEnvelope(stamp);

    ImageOf<PixelRgb> &outLeftImage  = outLeftPort.prepare();
    ImageOf<PixelRgb> &outRightImage = outRightPort.prepare();

    inWidth  = inputImage->width();
    inHeight = inputImage->height();

    if(horizontal)  // input image is horizontally aligned
    {
        outWidth  = inWidth/2;
        outHeight = inHeight;
    }
    else
    {
        outWidth  = inWidth;
        outHeight = inHeight/2;
    }

    outLeftImage.setQuantum(inputImage->getQuantum());
    outRightImage.setQuantum(inputImage->getQuantum());
    outLeftImage.resize(outWidth, outHeight);
    outRightImage.resize(outWidth, outHeight);

    // alloc and compute some vars for efficency
    int h2, w2;
    unsigned char *pixelLeft, *pixelRight;
    unsigned char *pixelInputL, *pixelInputR;
    unsigned char *pixelInput = inputImage->getRawImage();
    int dualImage_rowSizeByte = inputImage->getRowSize();
    int singleImage_rowSizeByte = outLeftImage.getRowSize();
    int singleImage_wholeSizeByte = outWidth * outHeight * outLeftImage.getPixelSize();

    static int counter = 0;
    static double start = 0;
    start = yarp::os::Time::now();

    switch(method)
    {
        case 0: // pixel by pixel
        {
            if(horizontal)
            {
                for(int h=0; h<outHeight; h++)
                {
                    for(int w1=0; w1<outWidth; w1++)
                    {
                        w2 = w1+outWidth;
                        pixelLeft = outLeftImage.getPixelAddress(w1, h);
                        pixelLeft[0] = *(inputImage->getPixelAddress(w1, h)+0);
                        pixelLeft[1] = *(inputImage->getPixelAddress(w1, h)+1);
                        pixelLeft[2] = *(inputImage->getPixelAddress(w1, h)+2);

                        pixelRight = outRightImage.getPixelAddress(w1, h);
                        pixelRight[0] = *(inputImage->getPixelAddress(w2, h)+0);
                        pixelRight[1] = *(inputImage->getPixelAddress(w2, h)+1);
                        pixelRight[2] = *(inputImage->getPixelAddress(w2, h)+2);
                    }
                }
            }
            else
            {
                for(int h1=0; h1<outHeight; h1++)
                {
                    for(int w=0; w<outWidth; w++)
                    {
                        h2 = h1+outHeight;
                        pixelLeft = outLeftImage.getPixelAddress(w, h1);
                        pixelLeft[0] = *(inputImage->getPixelAddress(w, h1)+0);
                        pixelLeft[1] = *(inputImage->getPixelAddress(w, h1)+1);
                        pixelLeft[2] = *(inputImage->getPixelAddress(w, h1)+2);

                        pixelRight = outRightImage.getPixelAddress(w, h1);
                        pixelRight[0] = *(inputImage->getPixelAddress(w, h2)+0);
                        pixelRight[1] = *(inputImage->getPixelAddress(w, h2)+1);
                        pixelRight[2] = *(inputImage->getPixelAddress(w, h2)+2);
                    }
                }
            }
        } break;

        case 1: // pixel by pixel, a bit better
        {
            if(horizontal)
            {
                pixelLeft  = outLeftImage.getRawImage();
                pixelRight = outRightImage.getRawImage();

                pixelInputL = pixelInput;
                pixelInputR = pixelInput+singleImage_rowSizeByte;
                for(int h=0, idx=0, idx2=0; h<outHeight; h++)
                {
                    for(int w=0; w<outWidth; w++)
                    {
                        pixelLeft[idx++] = *(pixelInputL++);
                        pixelLeft[idx++] = *(pixelInputL++);
                        pixelLeft[idx++] = *(pixelInputL++);

                        pixelRight[idx2++] = *(pixelInputR++);
                        pixelRight[idx2++] = *(pixelInputR++);
                        pixelRight[idx2++] = *(pixelInputR++);
                    }
                    pixelInputL += singleImage_rowSizeByte;
                    pixelInputR += singleImage_rowSizeByte;
                }
            }
            else
            {

            }
        } break;

        case 2: // line by line
        {
            if(horizontal)
            {
                pixelLeft  = outLeftImage.getRawImage();
                pixelRight = outRightImage.getRawImage();

                for(int h=0; h<inHeight; h++)
                {
                    memcpy(pixelLeft  + h*singleImage_rowSizeByte, pixelInput,                          singleImage_rowSizeByte);
                    memcpy(pixelRight + h*singleImage_rowSizeByte, pixelInput+=singleImage_rowSizeByte, singleImage_rowSizeByte);
                    pixelInput+= dualImage_rowSizeByte/2;
                }
            }
            else
            {
                pixelLeft  = outLeftImage.getRawImage();
                pixelRight = outRightImage.getRawImage();
                pixelInputL = pixelInput;
                pixelInputR = pixelInput+singleImage_wholeSizeByte;

                for(int h=0; h<outHeight; h++)
                {
                    memcpy(pixelLeft  + h*singleImage_rowSizeByte, pixelInputL, singleImage_rowSizeByte);
                    memcpy(pixelRight + h*singleImage_rowSizeByte, pixelInputR, singleImage_rowSizeByte);
                    pixelInputL+= singleImage_rowSizeByte;
                    pixelInputR+= singleImage_rowSizeByte;
                }
            }
        } break;

        case 3: // whole image, only if input image is vertically aligned
        {
            if(horizontal)
            {
                yError() << "Cannot use this copy method with horizontally aligned source image.";
            }
            else
            {
                pixelLeft  = outLeftImage.getRawImage();
                pixelRight = outRightImage.getRawImage();

                memcpy(pixelLeft,  pixelInput,                            singleImage_wholeSizeByte);
                memcpy(pixelRight, pixelInput+ singleImage_wholeSizeByte, singleImage_wholeSizeByte);
            }
        } break;

        default:
        {
            yError() << " @line " << __LINE__ << "unhandled switch case, we should not be here!";
        }
    }

    static double end = 0;
    static double elapsed = 0;
    end = yarp::os::Time::now();
    elapsed += (end-start);

    counter++;
    if((counter % 100) == 0)
    {
        yInfo() << "Elapsed time: " << elapsed;
        elapsed = 0;
    }

    outLeftPort.setEnvelope(stamp);
    outRightPort.setEnvelope(stamp);

    outLeftPort.write();
    outRightPort.write();
    return true;
}