Beispiel #1
0
void CamCalibPort::onRead(ImageOf<PixelRgb> &yrpImgIn)
{
    double t=Time::now();

    yarp::os::Stamp s;
    this->getEnvelope(s);
    double time = s.getTime();

    if (!updatePose(time)) {
        return;
    }

    unsigned char *pixel = yrpImgIn.getPixelAddress(0, 0);
    double *stamp = reinterpret_cast<double*>(pixel);
    double backdoorTime = stamp[0];
    double backdoorRoll = stamp[1];
    double backdoorPitch = stamp[2];
    double backdoorYaw = stamp[3];

    if (time != backdoorTime) {
        yWarning() << "Backdoor time:" << backdoorTime << "Imu time:" << time << "diff:" << (backdoorTime - time);
    }

    Bottle& b = rpyPort.prepare();
    b.clear();
    b.addDouble(roll);
    b.addDouble(pitch);
    b.addDouble(yaw);
    b.addDouble(backdoorRoll);
    b.addDouble(backdoorPitch);
    b.addDouble(backdoorYaw);
    b.addDouble(backdoorRoll - roll);
    b.addDouble(backdoorPitch - pitch);
    b.addDouble(backdoorYaw - yaw);
    rpyPort.write();



    // execute calibration
    if (portImgOut!=NULL) {
        yarp::sig::ImageOf<PixelRgb> &yrpImgOut=portImgOut->prepare();

        if (verbose) {
            yDebug("received input image after %g [s] ... ",t-t0);
        }

        double t1=Time::now();

        if (calibTool!=NULL) {
            calibTool->apply(yrpImgIn,yrpImgOut);

            for (int r =0; r <yrpImgOut.height(); r++) {
                for (int c=0; c<yrpImgOut.width(); c++) {
                    unsigned char *pixel = yrpImgOut.getPixelAddress(c,r);
                    double mean = (1.0/3.0)*(pixel[0]+pixel[1]+pixel[2]);

                    for(int i=0; i<3; i++) {
                        double s=pixel[i]-mean;
                        double sn=currSat*s;
                        sn+=mean;

                        if(sn<0.0)
                            sn=0.0;
                        else if(sn>255.0)
                            sn=255.0;

                        pixel[i]=(unsigned char)sn;
                    }
                }
            }

            if (verbose)
                yDebug("calibrated in %g [s]\n",Time::now()-t1);
        } else {
            yrpImgOut=yrpImgIn;

            if (verbose)
                yDebug("just copied in %g [s]\n",Time::now()-t1);
        }

        m.lock();

        //timestamp propagation
        //yarp::os::Stamp stamp;
        //BufferedPort<ImageOf<PixelRgb> >::getEnvelope(stamp);
        //portImgOut->setEnvelope(stamp);

        Bottle pose;
        pose.addDouble(roll);
        pose.addDouble(pitch);
        pose.addDouble(yaw);
        portImgOut->setEnvelope(pose);

        portImgOut->writeStrict();
        m.unlock();
    }

    t0=t;
}
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;
}