Esempio n. 1
0
    void onRead(T &obj) override
    {
        if (++cnt==dwnsample)
        {
            if (firstIncomingData)
            {
                yInfo() << "Incoming data detected";
                firstIncomingData=false;
            }

            DumpItem item;
            Stamp info;

            BufferedPort<T>::getEnvelope(info);
            item.seqNumber=info.getCount();

            if (txTime || (info.isValid() && !rxTime))
                item.timeStamp.setTxStamp(info.getTime());

            if (rxTime || !info.isValid())
                item.timeStamp.setRxStamp(Time::now());

            item.obj=factory(obj);

            buf.lock();
            buf.push_back(item);
            buf.unlock();

            cnt=0;
        }
    }
Esempio n. 2
0
void ThreeDModule::getEncoderPositions(double *headjnt_pos, double *torsojnt_pos, Stamp stamp) {	
	mutex.wait();
		
	if(stamp.getTime() == 0) {
		std::cout << "WARNING: No timestamp found in this bottle!! check your implementation!! " << std::endl;
		stamp.update();
	}
	
	// store the current (start) indices in the circular buffer
	int tIdx = torsoIdx;
	int hIdx = headIdx;
	
	// helpers
	int idxH = -1, idxT = -1;
	int hH, hT;
	double smallestDiffH = 10000;
	double smallestDiffT = 10000;	
	double tDiff;

	// the bottles to be copied out of the circ buffer
	Bottle head, torso;
	
	// going through the circular buffer
    idxH = 1;
    idxT = 1;
//	for(int k = 0; k < LIST_LENGTH; k++) {
//		hT = (LIST_LENGTH + tIdx - k) % LIST_LENGTH;	
//		hH = (LIST_LENGTH + hIdx - k) % LIST_LENGTH;
//		
//		tDiff = fabs(stamp.getTime() - torsoStamp[hT].getTime());
//		std::cout << stamp.getTime() << " idx: " << hT <<  " getTime: " << torsoStamp[hT].getTime() << ", diffT: "<< tDiff << std::endl;		
//		if( tDiff < smallestDiffT ) {
//			idxT = hT;				
//			smallestDiffT = tDiff;
//		}
//
//		tDiff = fabs(stamp.getTime() - torsoStamp[hH].getTime());
//		if( tDiff < smallestDiffH ) { 
//			idxH = hH;		
//			smallestDiffH = tDiff;
//		}
//	}
	
	if( idxH != -1 ) head = headState[idxH];	
	if( idxT != -1 ) torso = torsoState[idxT];
	std::cout <<  "idxT: " << idxT << "/" << idxT-tIdx <<  " val: " << torso.toString() <<  std::endl;			
	std::cout <<  "idxH: " << idxH << "/" << idxH-hIdx << " val: " << head.toString() <<  std::endl;				
	
	for(int i = 0; i < 6; i++) {
		headjnt_pos[i] = head.get(i).asDouble();
		if(i < 3) torsojnt_pos[i] = torso.get(i).asDouble();
	}
	
	mutex.post();
}
Esempio n. 3
0
void Rangefinder2DInputPortProcessor::onRead(yarp::os::Bottle &b)
{
    now=SystemClock::nowSystem();
    mutex.wait();

    if (count>0)
    {
        double tmpDT=now-prev;
        deltaT+=tmpDT;
        if (tmpDT>deltaTMax)
            deltaTMax=tmpDT;
        if (tmpDT<deltaTMin)
            deltaTMin=tmpDT;

        //compare network time
        if (tmpDT*1000<LASER_TIMEOUT)
        {
            state = b.get(1).asInt();
        }
        else
        {
            state = IRangefinder2D::DEVICE_TIMEOUT;
        }
    }

    prev=now;
    count++;

    lastBottle=b;
    Stamp newStamp;
    getEnvelope(newStamp);

    //initialialization (first received data)
    if (lastStamp.isValid()==false)
    {
        lastStamp = newStamp;
    }

    //now compare timestamps
    if ((1000*(newStamp.getTime()-lastStamp.getTime()))<LASER_TIMEOUT)
    {
        state = b.get(1).asInt();
    }
    else
    {
        state = IRangefinder2D::DEVICE_TIMEOUT;
    }
    lastStamp = newStamp;

    mutex.post();
}
void InputPortProcessor::onRead(yarp::sig::Vector &v)
{
    now=Time::now();
    mutex.wait();

    if (count>0)
    {
        double tmpDT=now-prev;
        deltaT+=tmpDT;
        if (tmpDT>deltaTMax)
            deltaTMax=tmpDT;
        if (tmpDT<deltaTMin)
            deltaTMin=tmpDT;

        //compare network time
        if (tmpDT*1000<ANALOG_TIMEOUT)
        {
            state=IAnalogSensor::AS_OK;
        }
        else
        {
            state=IAnalogSensor::AS_TIMEOUT;
        }
    }

    prev=now;
    count++;

    lastVector=v;
    Stamp newStamp;
    getEnvelope(newStamp);

    //initialialization (first received data)
    if (lastStamp.isValid()==false)
    {
        lastStamp = newStamp;
    }

    //now compare timestamps
    if ((1000*(newStamp.getTime()-lastStamp.getTime()))<ANALOG_TIMEOUT)
    {
        state=IAnalogSensor::AS_OK;
    }
    else
    {
        state=IAnalogSensor::AS_TIMEOUT;
    }
    lastStamp = newStamp;

    mutex.post();
}
Esempio n. 5
0
    bool updateModule()
    {
        Vector *imuData=iPort.read();
        if (imuData==NULL)
            return false;

        Stamp stamp;
        iPort.getEnvelope(stamp);

        double t0=Time::now();
        Vector gyro=imuData->subVector(6,8);
        Vector gyro_filt=gyroFilt.filt(gyro);

        gyro-=gyroBias;
        gyro_filt-=gyroBias;

        Vector mag_filt=magFilt.filt(imuData->subVector(9,11));
        double magVel=norm(velEst.estimate(AWPolyElement(mag_filt,stamp.getTime())));

        adaptGyroBias=adaptGyroBias?(magVel<mag_vel_thres_up):(magVel<mag_vel_thres_down);
        gyroBias=biasInt.integrate(adaptGyroBias?gyro_filt:Vector(3,0.0));
        double dt=Time::now()-t0;

        if (oPort.getOutputCount()>0)
        {
            Vector &outData=oPort.prepare();
            outData=*imuData;
            outData.setSubvector(6,gyro);
            oPort.setEnvelope(stamp);
            oPort.write();
        }

        if (bPort.getOutputCount()>0)
        {
            bPort.prepare()=gyroBias;
            bPort.setEnvelope(stamp);
            bPort.write();
        }

        if (verbose)
        {
            yInfo("magVel   = %g => [%s]",magVel,adaptGyroBias?"adapt-gyroBias":"no-adaption");
            yInfo("gyro     = %s",gyro.toString(3,3).c_str());
            yInfo("gyroBias = %s",gyroBias.toString(3,3).c_str());
            yInfo("dt       = %.0f [us]",dt*1e6);
            yInfo("\n");
        }

        return true;
    }
void stereoCalibThread::stereoCalibRun()
{

    imageL=new ImageOf<PixelRgb>;
    imageR=new ImageOf<PixelRgb>;

    Stamp TSLeft;
    Stamp TSRight;

    bool initL=false;
    bool initR=false;

    int count=1;
    Size boardSize, imageSize;
    boardSize.width=this->boardWidth;
    boardSize.height=this->boardHeight;


   while (!isStopping()) { 
        ImageOf<PixelRgb> *tmpL = imagePortInLeft.read(false);
        ImageOf<PixelRgb> *tmpR = imagePortInRight.read(false);

        if(tmpL!=NULL)
        {
            *imageL=*tmpL;
            imagePortInLeft.getEnvelope(TSLeft);
            initL=true;
        }
        if(tmpR!=NULL) 
        {
            *imageR=*tmpR;
            imagePortInRight.getEnvelope(TSRight);
            initR=true;
        }

        if(initL && initR && checkTS(TSLeft.getTime(),TSRight.getTime())){

            bool foundL=false;
            bool foundR=false;

            mutex->wait();
            if(startCalibration>0) {

                string pathImg=imageDir;
                preparePath(pathImg.c_str(), pathL,pathR,count);
                string iml(pathL);
                string imr(pathR);
                imgL= (IplImage*) imageL->getIplImage();
                imgR= (IplImage*) imageR->getIplImage();
                Mat Left(imgL);
                Mat Right(imgR);


                std::vector<Point2f> pointbufL;
                std::vector<Point2f> pointbufR;
                if(boardType == "CIRCLES_GRID") {
                    foundL = findCirclesGridDefault(Left, boardSize, pointbufL, CALIB_CB_SYMMETRIC_GRID  | CALIB_CB_CLUSTERING);
                    foundR = findCirclesGridDefault(Right, boardSize, pointbufR, CALIB_CB_SYMMETRIC_GRID  | CALIB_CB_CLUSTERING);
                } else if(boardType == "ASYMMETRIC_CIRCLES_GRID") {
                    foundL = findCirclesGridDefault(Left, boardSize, pointbufL, CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING);
                    foundR = findCirclesGridDefault(Right, boardSize, pointbufR, CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING);
                } else {
                    foundL = findChessboardCorners( Left, boardSize, pointbufL, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
                    foundR = findChessboardCorners( Right, boardSize, pointbufR, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
                }
                
                if(foundL && foundR) {
                        cvCvtColor(imgL,imgL,CV_RGB2BGR);
                        cvCvtColor(imgR,imgR, CV_RGB2BGR);
                        saveStereoImage(pathImg.c_str(),imgL,imgR,count);

                        imageListR.push_back(imr);
                        imageListL.push_back(iml);
                        imageListLR.push_back(iml);
                        imageListLR.push_back(imr);
                        Mat cL(pointbufL);
                        Mat cR(pointbufR);
                        drawChessboardCorners(Left, boardSize, cL, foundL);
                        drawChessboardCorners(Right, boardSize, cR, foundR);
                        count++;
                }

                if(count>numOfPairs) {
                    fprintf(stdout," Running Left Camera Calibration... \n");
                    monoCalibration(imageListL,this->boardWidth,this->boardHeight,this->Kleft,this->DistL);

                    fprintf(stdout," Running Right Camera Calibration... \n");
                    monoCalibration(imageListR,this->boardWidth,this->boardHeight,this->Kright,this->DistR);

                    stereoCalibration(imageListLR, this->boardWidth,this->boardHeight,this->squareSize);

                    fprintf(stdout," Saving Calibration Results... \n");
                    updateIntrinsics(imgL->width,imgL->height,Kright.at<double>(0,0),Kright.at<double>(1,1),Kright.at<double>(0,2),Kright.at<double>(1,2),DistR.at<double>(0,0),DistR.at<double>(0,1),DistR.at<double>(0,2),DistR.at<double>(0,3),"CAMERA_CALIBRATION_RIGHT");
                    updateIntrinsics(imgL->width,imgL->height,Kleft.at<double>(0,0),Kleft.at<double>(1,1),Kleft.at<double>(0,2),Kleft.at<double>(1,2),DistL.at<double>(0,0),DistL.at<double>(0,1),DistL.at<double>(0,2),DistL.at<double>(0,3),"CAMERA_CALIBRATION_LEFT");

                    Mat Rot=Mat::eye(3,3,CV_64FC1);
                    Mat Tr=Mat::zeros(3,1,CV_64FC1);

                    updateExtrinsics(this->R,this->T,"STEREO_DISPARITY");

                    fprintf(stdout, "Calibration Results Saved in %s \n", camCalibFile.c_str());

                    startCalibration=0;
                    count=1;
                    imageListR.clear();
                    imageListL.clear();
                    imageListLR.clear();
                }


            }
            mutex->post();
            ImageOf<PixelRgb>& outimL=outPortLeft.prepare();
            outimL=*imageL;
            outPortLeft.write();

            ImageOf<PixelRgb>& outimR=outPortRight.prepare();
            outimR=*imageR;
            outPortRight.write();

            if(foundL && foundR && startCalibration==1)
                Time::delay(2.0);
            initL=initR=false;
            cout.flush();
        }
   }

   delete imageL;
   delete imageR;
 }
Esempio n. 7
0
int main(int argc, char *argv[]) {

	//Declair yarp
	Network yarp;
	if (!yarp.checkNetwork())
	{
		printf("YARP server not available!\n");
		return -1;
	}

	//Creating a Resiving yarp port
	BufferedPort<Sound> bufferPort;
	bufferPort.open("/receiver");
	if (!Network::exists("/receiver")) {
		printf("receiver not exists \n");
		return -1;
	}

	//Connecting the resiving audio port with the sender on the pc104
	Network::connect("/sender", "/receiver");
	if (!Network::isConnected("/sender", "/receiver")) {
		printf("no connection \n");
		return -1;
	}

	double empty[samplingBuffer *4] = {0}; //plus 2 for time and sample stamps

	//Setup for memory mapping
	FILE *fid;
	fid = fopen("/tmp/AudioMemMap.tmp", "w");
	fwrite(empty, sizeof(double), samplingBuffer * 4, fid); 
	fclose(fid);
	int mappedFileID;
	mappedFileID = open("/tmp/AudioMemMap.tmp", O_RDWR);
	double *mappedAudioData;
	mappedAudioData = (double *)mmap(0, memoryMapSize_bytes, PROT_WRITE, MAP_SHARED , mappedFileID, 0);
	

	//Main loop that maps the sound to the memory mapped region delaired above
	Sound* s;
	Stamp ts;
	while (true) {
		//This is a blocking read
		s = bufferPort.read(true);
		bufferPort.getEnvelope(ts);
		printf("count:%d time:%f \n", ts.getCount(), ts.getTime());
		int e0 = ts.getCount();
		double e1 = ts.getTime();
		int row = 0;
		for (int col = 0 ; col < samplingBuffer; col++) {
			NetInt16 temp_c = (NetInt16) s->get(col, 0);
			NetInt16 temp_d = (NetInt16) s->get(col, 1);
			mappedAudioData[row]        	= (double) 	temp_c / normDivid ;
			mappedAudioData[row + 1] 		= (double) 	temp_d / normDivid;
			mappedAudioData[row + 2] 		= (double) 	(e0 * samplingBuffer) + col;
			mappedAudioData[row + 3]		= (double) 	(e1 + col * samplePeriod);
			row += 4;

		}
	}
	return 0;
}
Esempio n. 8
0
bool GBSegmModule::updateModule()
{
    ImageOf<PixelRgb> *yrpImgIn;
    static int cycles = 0;

    yrpImgIn = _imgPort.read();
    if (yrpImgIn == NULL)   // this is the case if module is requested to quit while waiting for image
        return true;

    bool use_private_stamp;
    Stamp s;
    if(!_imgPort.getEnvelope(s))
    {
            cout << "No stamp found in input image. Will use private stamp" << endl;
            use_private_stamp = true;
    }
    else
    {
            cout << "Received image #" << s.getCount() << " generated at time " << s.getTime() << endl;
            use_private_stamp = false;
    }

    if(cycles == 0)
            _timestart = yarp::os::Time::now();
    cycles++;

    //IplImage *iplimg = (IplImage*)yrpImgIn->getIplImage();
 cout << "converting image of size " << yrpImgIn->width() << yrpImgIn->height() <<" to size" << input->width() << input->height() << endl;
    YarpImageToRGBImage(input, yrpImgIn);
    cout << "converted" << endl;
    segMutex.wait();
    if(seg)
        delete seg;
    seg=segment_image(input, sigma, k, min_size, &num_components);
    segMutex.post();
    
    cout << "processed" << endl;
    //prepare timestamps
    if(use_private_stamp)
    {
        _stamp.update();
        _viewPort.setEnvelope(_stamp);
    }
    else
    {
        _viewPort.setEnvelope(s);
    }



    ImageOf<PixelRgb> &yrpImgView = _viewPort.prepare();
    //Rescale image if required

    yrpImgView.resize(seg->width(), seg->height());
    RGBImageToYarpImage(seg, &yrpImgView);
    _viewPort.write();


    //report the frame rate
    if(cycles % 100 == 0)
    {
            double cps = ((double)cycles)/(yarp::os::Time::now() - _timestart);
            printf("fps: %02.2f\n", cps);
    }
    return true;
}
Esempio n. 9
0
//  Generate new values 
void DataPlot::timerEvent(QTimerEvent *)
{
    for (int k = 0; k < numberOfInputPorts; k++)
        {
            int timeout = 0;
            int maxTimeout = 2;
            Bottle *b = NULL;
            
            while(b==NULL && timeout < maxTimeout) 
                {
                    b = inputPorts[k].read(false);
                    Time::delay(0.005);
                    timeout++;
                }
            if (timeout==maxTimeout)
                {
                    if(VERBOSE) fprintf(stderr, "MESSAGE: Couldn't receive data. Going to put a zero! \n");
                    for(int i = 0; i < numberOfPlots[k]; i++)
                        {
                            d_y[k][i][PLOT_SIZE - 1] = 0;
                            for ( int j = 0; j < PLOT_SIZE - 1; j++ )
                                {
                                    d_y[k][i][j] = d_y[k][i][j+1];
                                }
                        }
                }
            else
                {
                    for(int i = 0; i < numberOfPlots[k]; i++)
                        {

                            Stamp stmp;
                            inputPorts[k].getEnvelope(stmp);
#ifdef ENABLE_REALTIME
                            if (stmp.isValid()) 
                                d_x_real_time[PLOT_SIZE - 1] = (stmp.getTime() - initialTime);                            
                            
                            for ( int j = 0; j < PLOT_SIZE - 1; j++ )
                                {
                                    if(realTime)
                                        d_x_real_time[j] =  d_x_real_time[j+1];
                                }
#endif
                            if (b==NULL)
                                d_y[k][i][PLOT_SIZE - 1] = 0;
                            else
                                if (b->size()-1 >= index[k][i])
                                    {
                                        d_y[k][i][PLOT_SIZE - 1] = b->get(index[k][i]).asDouble();
                                        //if(VERBOSE) fprintf(stderr, "MESSAGE: Getting from port %d the index %d\n", k, index[k][i]);
                                    }
                            // y moves from left to right:
                            // Shift y array right and assign new value to y[0].
                            
                            for ( int j = 0; j < PLOT_SIZE - 1; j++ )
                                {
                                    d_y[k][i][j] = d_y[k][i][j+1];
                                }
                            
                            // update the display
                            //setAxisScale(QwtPlot::yLeft, min, max);
#ifdef ENABLE_REALTIME                            
                            if(numberAcquiredData==PLOT_SIZE && realTime)
                                {
                                    if(VERBOSE) fprintf(stderr, "MESSAGE: switching to real time\n");
                                    
                                    QwtPlotCurve *timeBasedCurve = new QwtPlotCurve("Data");
                                    timeBasedCurve->attach(this);
                                    timeBasedCurve->setRawData(d_x_real_time, d_y[k][i], PLOT_SIZE);
                                    timeBasedCurve->setPen(coloredPens[k%NUMBER_OF_LIN][i%NUMBER_OF_COL]);
                                    nonTimeBasedCurve[k][i].attach(NULL);
                                    
                                    //Set title
                                    char cTitle[256];
                                    sprintf(cTitle, "Data(%d)", index[k][i]);
                                    QwtText curveTitle(cTitle, QwtText::PlainText);
                                    curveTitle.setFont(plotFont);
                                    timeBasedCurve->setTitle(curveTitle); 
                                }
#endif
                        }
                }
        }
    if (acquire)
        replot();

    numberAcquiredData++;
    //if(VERBOSE) fprintf(stderr, "Number of acquired data is %d\n", numberAcquiredData);
    //QSize plotSize= this->sizeHint();
    //if(VERBOSE) fprintf(stderr, "Hint is: hInt=%d, vInt=%d\n", plotSize.height(), plotSize.width()); 

	static double before = Time::now();	
	double now = Time::now();
	static double meanEffectiveTime = 0;

	double currentEffectiveTime = (now - before)*1000.0;
	if (numberAcquiredData >= 2)
		meanEffectiveTime = (meanEffectiveTime*(numberAcquiredData-2) + currentEffectiveTime)/(numberAcquiredData-1);
	//if(VERBOSE) fprintf(stderr, "Iteration %d: Current time is %f and mean is %f\n", numberAcquiredData, currentEffectiveTime, meanEffectiveTime);	
	if (currentEffectiveTime*0.5 > d_interval)
	{
		if(VERBOSE) fprintf(stderr, "Real Timer is %f but I was supposed to run at %d ms. Mean is: %f \n", currentEffectiveTime, d_interval, meanEffectiveTime);	
	    if(VERBOSE) fprintf(stderr, "You should slow down to %d ms \n", (int) (meanEffectiveTime * 2));	
        //setTimerInterval((int) (meanEffectiveTime * 2));
	}

	before = now;

}
Esempio n. 10
0
void DataPlot::initSignalDimensions()
{
    if(VERBOSE) fprintf(stderr, "MESSAGE: Will now initialize the signal dimensions\n");
    //getting info on the connected port
    nonTimeBasedCurve = new QwtPlotCurve*[numberOfInputPorts];
    for (int k = 0; k < numberOfInputPorts; k++)
        {
            Bottle *b = NULL;
            int timeout = 0;
            const int maxTimeout = 1000;
            while(b==NULL && timeout < maxTimeout) 
                {
                    b = inputPorts[k].read(false);
                    Time::delay(0.005);
                    timeout++;
                }
            if (timeout==maxTimeout)
                {
                    if(VERBOSE) fprintf(stderr, "MESSAGE: Couldn't receive data. Going to put a zero! \n");
                    realTime = false;
                    //  Initialize data
                    for (int j = 0; j< numberOfPlots[k]; j++)
                        {
                            for (int i = 0; i < PLOT_SIZE; i++)
                                {
                                    d_x[i] = i;     // time axis
                                    //if(VERBOSE) fprintf(stderr, "MESSAGE: (%d, %d)\n", j, i);
                                    d_y[k][j][i] = 0;
                                }
                        }
                }
            else
                {
                    if(VERBOSE) fprintf(stderr, "MESSAGE: Will now try real time!\n");
                    inputVectorSize = b->size();
                    Stamp stmp;
                    inputPorts[k].getEnvelope(stmp);
                    if (stmp.isValid())
                        {     
                            if(VERBOSE) fprintf(stderr, "MESSAGE: will use real time!\n");
                            realTime = true;
                            initialTime = stmp.getTime();
                        }
                
                    //  Initialize data
                    for (int j = 0; j< numberOfPlots[k]; j++)
                        {
                            if (b->size()-1 < index[k][j])
                                if(VERBOSE) fprintf(stderr, "WARNING: will plot some zeros since the accessed index exceed the input vector dimensions!\n");
                            for (int i = 0; i < PLOT_SIZE; i++)
                                {
                                    d_x[i] = i;     // time axis
#ifdef ENABLE_REALTIME
                                    if (realTime)
                                        d_x_real_time[i] = i;
#endif
                                    //if(VERBOSE) fprintf(stderr, "MESSAGE: (%d, %d)\n", j, i);
                                    d_y[k][j][i] = 0;
                                }
                        }
                }

            //if(VERBOSE) fprintf(stderr, "MESSAGE: initializing plot datas!\n");
            // Assign a title
            insertLegend(new QwtLegend(), QwtPlot::BottomLegend);

            nonTimeBasedCurve[k] = new QwtPlotCurve[numberOfPlots[k]];
            for(int i=0; i < numberOfPlots[k]; i++)
                {
                    //Set title
                    char cTitle[256];
                    sprintf(cTitle, "Data(%d)", index[k][i]);
                    QwtText curveTitle(cTitle, QwtText::PlainText);
                    curveTitle.setFont(plotFont);
                    nonTimeBasedCurve[k][i].setTitle(curveTitle);

                    //if(VERBOSE) fprintf(stderr, "MESSAGE: Will now initialize the plot %d\n", index[i]);
                    // Insert new curves
                    nonTimeBasedCurve[k][i].attach(this);

                    // Set curve styles
                    nonTimeBasedCurve[k][i].setPen(coloredPens[k%NUMBER_OF_LIN][i%NUMBER_OF_COL]);

                    // Attach (don't copy) data. Both curves use the same x array.
                    nonTimeBasedCurve[k][i].setRawData(d_x, d_y[k][i], PLOT_SIZE);
                }

            // Axis 
            QwtText axisTitle("Time/seconds");
            axisTitle.setFont(plotFont);
            setAxisTitle(QwtPlot::xBottom, axisTitle);
            setAxisScale(QwtPlot::xBottom, 0, 100);
            setAxisFont(QwtPlot::xBottom, plotFont);

            setAxisTitle(QwtPlot::yLeft, "Values");
            //setAxisScale(QwtPlot::yLeft, -1.5, 1.5);
            setAxisAutoScale(QwtPlot::yLeft);
            setAxisAutoScale(QwtPlot::xBottom);
            setAxisFont(QwtPlot::yLeft, plotFont);

            setTimerInterval(50.0); 
        }
    //if(VERBOSE) fprintf(stderr, "MESSAGE: plot intialized!\n");
}
Esempio n. 11
0
bool EdisonSegmModule::updateModule()
{
    ImageOf<PixelRgb> *yrpImgIn;
    static int cycles = 0;

    yrpImgIn = _imgPort.read();
    if (yrpImgIn == NULL)   // this is the case if module is requested to quit while waiting for image
        return true;

    bool use_private_stamp;
    Stamp s;
    if(!_imgPort.getEnvelope(s))
    {
            cout << "No stamp found in input image. Will use private stamp" << endl;
            use_private_stamp = true;
    }
    else
    {
            cout << "Received image #" << s.getCount() << " generated at time " << s.getTime() << endl;
            use_private_stamp = false;
    }

    if(cycles == 0)
            _timestart = yarp::os::Time::now();
    cycles++;

    IplImage *iplimg = (IplImage*)yrpImgIn->getIplImage();

    //computing the ROI to crop the image
	/*struct _IplROI roi;
	roi.coi = 0; // all channels are selected
	roi.height = height_;
	roi.width = width_;
	roi.xOffset = ( orig_width_ - width_ ) / 2;
	roi.yOffset = ( orig_height_ - height_ ) / 2;*/
	
	//copying roi data to buffer
	/*iplimg->roi = &roi;
	cvCopy( iplimg, inputImage.getIplImage());*/

    //Rescale image if required
    if( (width_ != orig_width_) || (height_ != orig_height_ ) )
            cvResize(iplimg, inputImage.getIplImage(), CV_INTER_NN);
    else
            cvCopy( iplimg, inputImage.getIplImage());

    double edgetime = yarp::os::Time::now();
    //compute gradient and confidence maps
    BgEdgeDetect edgeDetector(gradWindRad);
    BgImage bgImage;
    bgImage.SetImage(inputImage_, width_, height_, true);
    edgeDetector.ComputeEdgeInfo(&bgImage, confMap_, gradMap_);
    //compute the weigth map
    for(int i = 0; i < width_*height_; i++) {
      if(gradMap_[i] > 0.02) {
		weightMap_[i] = mixture*gradMap_[i] + (1 - mixture)*confMap_[i];
      } else {
		weightMap_[i] = 0;
      }
    }
	///////////////////////////// This block can be parallelized
	cout << "Edge computation Time (ms): " << (yarp::os::Time::now() - edgetime)*1000.0 << endl;

	msImageProcessor iProc;
	if( dim_ == 3 )
		iProc.DefineImage(inputImage_, COLOR, height_, width_);
	else
	{	
		cvCvtColor(inputImage.getIplImage(), inputHsv.getIplImage(), CV_RGB2HSV);
		cvSplit(inputHsv.getIplImage(), inputHue.getIplImage(), 0, 0, 0);
		iProc.DefineImage(inputHue_, GRAYSCALE, height_, width_);
	}
	if(iProc.ErrorStatus) {
		cout << "MeanShift Error" << endl;
		return false;
	}
	iProc.SetWeightMap(weightMap_, threshold);
	if(iProc.ErrorStatus) {
		cout << "MeanShift Error" << endl;
		return false;
	}


	double filtertime = yarp::os::Time::now();
	iProc.Filter(sigmaS, sigmaR, speedup);
        if(iProc.ErrorStatus) {
		cout << "MeanShift Error" << endl;
		return false;
	}
	cout << "Mean Shift Filter Computation Time (ms): " << (yarp::os::Time::now() - filtertime)*1000.0 << endl;


    //obtain the filtered image
    iProc.GetResults(filtImage_);
	if(iProc.ErrorStatus) {
		cout << "MeanShift Error" << endl;
		return false;
	}
    
    //fuse regions
	double fusetime = yarp::os::Time::now();
    iProc.FuseRegions(sigmaR, minRegion);
    if(iProc.ErrorStatus) {
		cout << "MeanShift Error" << endl;
		return false;
	}
	cout << "Region Fusion Computation Time (ms): " << (yarp::os::Time::now() - fusetime)*1000.0 << endl;

    //obtain the segmented image
    iProc.GetResults(segmImage_);
    if(iProc.ErrorStatus) {
		cout << "MeanShift Error" << endl;
		return false;
	}

	//define the boundaries - do not need this
	/*
	RegionList *regionList        = iProc.GetBoundaries();
	int        *regionIndeces     = regionList->GetRegionIndeces(0);
	int        numRegions         = regionList->GetNumRegions();
	numBoundaries_ = 0;
	for(int i = 0; i < numRegions; i++) {
		numBoundaries_ += regionList->GetRegionCount(i);
	}
	if(boundaries_) delete [] boundaries_;
	boundaries_ = new int [numBoundaries_];
	for(int i = 0; i < numBoundaries_; i++) {
		boundaries_[i] = regionIndeces[i];
	}*/
		
	int regionCount; // how many regions have been found
	int *labels = NULL; //pointer for the labels (should this be released in the end?) 
	float *modes; //pointer for the Luv values (should this be released in the end?) 
	int *modePointCounts; //the area of each region (should this be released in the end?) 

	regionCount = iProc.GetRegions(&labels, &modes, &modePointCounts);
	int *labelp = (int*)labelImage.getRawImage();
	for(int i = 0; i < width_*height_; i++)
		labelp[i] = labels[i];
	
	IplImage *labelint = (IplImage*)labelImage.getIplImage();
	IplImage *labelchar = (IplImage*)labelView.getIplImage();

	cvConvert(labelint, labelchar);

	//prepare timestamps
	if(use_private_stamp)
	{
		_stamp.update();
		_labelPort.setEnvelope(_stamp);
		_labelViewPort.setEnvelope(_stamp);
		_viewPort.setEnvelope(_stamp);
		_filtPort.setEnvelope(_stamp);
		_rawPort.setEnvelope(_stamp);
	}
	else
	{
		_labelPort.setEnvelope(s);
		_labelViewPort.setEnvelope(s);
		_viewPort.setEnvelope(s);
		_filtPort.setEnvelope(s);
		_rawPort.setEnvelope(s);
	}

	ImageOf<PixelInt> &yrpImgLabel = _labelPort.prepare();
	//Rescale image if required
	if( (width_ != orig_width_) || (height_ != orig_height_ ) )
	{
		yrpImgLabel.resize(orig_width_, orig_height_);
		cvResize(labelImage.getIplImage(), yrpImgLabel.getIplImage(), CV_INTER_NN);
	}
	else
		yrpImgLabel = labelImage;
	_labelPort.write();

	ImageOf<PixelMono> &yrpImgDebug = _labelViewPort.prepare();
	//Rescale image if required
	if( (width_ != orig_width_) || (height_ != orig_height_ ) )
	{
		yrpImgDebug.resize(orig_width_, orig_height_);
		cvResize(labelView.getIplImage(), yrpImgDebug.getIplImage(), CV_INTER_NN);
	}
	else
		yrpImgDebug = labelView;
	_labelViewPort.write();


	ImageOf<PixelRgb> &yrpFiltOut = _filtPort.prepare();
	//Rescale image if required
	if( (width_ != orig_width_) || (height_ != orig_height_ ) )
	{
		yrpFiltOut.resize(orig_width_, orig_height_);
		cvResize(filtImage.getIplImage(), yrpFiltOut.getIplImage(), CV_INTER_NN);
	}
	else
		yrpFiltOut = filtImage;
	_filtPort.write();

	ImageOf<PixelRgb> &yrpImgView = _viewPort.prepare();
	//Rescale image if required
	if( (width_ != orig_width_) || (height_ != orig_height_ ) )
	{
		yrpImgView.resize(orig_width_, orig_height_);
		cvResize(segmImage.getIplImage(), yrpImgView.getIplImage(), CV_INTER_NN);
	}
	else
		yrpImgView = segmImage;
	_viewPort.write();

	ImageOf<PixelRgb> &yrpImgOut = _rawPort.prepare();
	yrpImgOut = *yrpImgIn;
	_rawPort.write();


	//report the frame rate
	if(cycles % 100 == 0)
	{
		double cps = ((double)cycles)/(yarp::os::Time::now() - _timestart);
		printf("fps: %02.2f\n", cps);
	}
    return true;
}