C++ Bottle::pop方法代码示例

 void testStack() {
     report(0,"testing stack functionality...");
     Bottle bot;
     bot.addInt(10);
     bot.addInt(11);
     bot.addString("Foo");
     Bottle& bot2 = bot.addList();
     bot2.addInt(3);
     bot.addDouble(2.71828);
     checkTrue(bot.pop().isDouble(),"popping double");
     checkEqual(bot.size(),4,"bottle size decreased after pop");
     checkEqual(bot.pop().asList()->pop().asInt(),3,"popping list and nested int");
     checkEqual(bot.pop().asString().c_str(),"Foo", "popping string");
     Value val;
     val = bot.pop();
     checkTrue(val.isInt(), "popped value is of type int");
     checkEqual(val.asInt(), 11, "popped value is integer 11");
     val = bot.pop();
     checkTrue(val.isInt(), "popped value is of type int");
     checkEqual(val.asInt(), 10, "popped value is integer 10");
     val = bot.pop();
     checkTrue(bot.pop().isNull(), "empty bottle pops null");
     checkEqual(bot.size(),0,"bottle is empty after popping");
 }

int main()
{

Network yarp;

bool verbose=false;

string portname_leftHandNeurons = "/local/leftArmPoseNeuron";
Port leftHandNeuronsPort;
leftHandNeuronsPort.open(portname_leftHandNeurons.c_str());

string portname_testHandNeurons = "/leftHandPoseNeurons";
BufferedPort<Bottle> testHandNeuronsPort;
testHandNeuronsPort.open(portname_testHandNeurons.c_str());

string portname_leftHandVelocity = "/leftHandVelocity";
BufferedPort<Bottle> leftHandVelocityPort;
leftHandVelocityPort.open(portname_leftHandVelocity.c_str());
//neuronData.resize(3*sizeof(int));

//neuronData.resize(3);


string portname_icubLeftArmLocal = "/icubSim/world";
cout << verbose << endl;
cout << Network::connect(portname_leftHandNeurons.c_str(), portname_icubLeftArmLocal.c_str()) << endl;

yarp::sig::Vector vec_effEndTra;
yarp::sig::Vector vec_effEndRot;
vec_effEndTra.resize(3*sizeof(double));

int xMin, yMin, zMin, xMax, yMax, zMax;
xMin=-45; xMax=10; yMin=-8; yMax=40; zMin=-33; zMax=-7;
int numX = xMax-xMin+2;
int numY = yMax-yMin+2;
cout << "numX Y: " << numX << " " << numY << endl;
IzhikevicNeuron Neurons[numX][numY];

//double potential = 0;
long int spikeTime = 0;

double inputCurrent;

double initTime = Time::now();
//file_init_time << initTime << endl;

double t;
double noise;
double X = 0; 
double Y = 0;
double Z = 0;

double Xp, Yp, Zp, Xhat, Yhat, Zhat;
Xp=0; Yp=0; Zp=0;


Bottle writeToPort("world get lhand");
Bottle replyFromWorld;
while(true){
	
	leftHandNeuronsPort.write(writeToPort, replyFromWorld);
   // neuronData.clear();
	vec_effEndTra[2] = -(replyFromWorld.pop()).asDouble()-0.04;
	vec_effEndTra[1] = (replyFromWorld.pop()).asDouble()-0.5484;
	vec_effEndTra[0] = -(replyFromWorld.pop()).asDouble();

    if(verbose) cout << "position of end effector: " << vec_effEndTra.toString().c_str() << endl;

    
	X = (vec_effEndTra[0]*100);
	Y = (vec_effEndTra[1]*100);
	Z = (vec_effEndTra[2]*100);

	Xhat = X-Xp;
	Yhat = Y-Yp;
	Zhat = Z-Zp;
	

	for (int i = xMin; i < xMax; i=i+5){
		for (int j = yMin; j < yMax; j=j+5){
	
    			noise = rand()%10;
    			inputCurrent = 1000*(sqrt((Xhat*Xhat+Yhat*Yhat+Zhat*Zhat)))*exp(-sqrt((i-X)*(i-X)+(j-Y)*(j-Y))/2) + noise;

			t = abs(Time::now());

//cout << i-xMin << " " << j-yMin << " " << spikeTime << endl;

			Neurons[i-xMin][j-yMin].updatePotential(inputCurrent, t);
 //   double potential = Neurons[21][21].getPotential();
//cout << potential << endl;
			
    			spikeTime = Neurons[i-xMin][j-yMin].getLastSpikeTime();
			
    			
 //   file_endeffector << X << " " << Y << " " << spikeTime << endl;
cout << X << " " << Y << " " << Z << endl;

 //   dataToWrite = neuronData.data();
  //  cout << neuronData.size() << endl;
//.........这里部分代码省略.........

void StorerThread::run()
{
    mutex.wait();
    Bottle *bot = port_in_scores.read(false);

    if(bot==NULL || bot->size()<1)
    {
        mutex.post();
        return;
    }

    string true_class = bot->pop().asString().c_str();
    scores_buffer.push_back(*bot);

    //if the scores exceed a certain threshold clear its head
    while(scores_buffer.size()>bufferSize)
        scores_buffer.pop_front();

    if(scores_buffer.size()<1)
    {
        mutex.post();
        return;
    }

    int n_classes = scores_buffer.front().size();

    vector<double> class_avg(n_classes,0.0);
    vector<int> class_votes(n_classes,0);

    for(list<Bottle>::iterator score_itr=scores_buffer.begin(); score_itr!=scores_buffer.end(); score_itr++)
    {
        double max_score=-1000.0;
        int max_idx;
        for(int class_idx=0; class_idx<n_classes; class_idx++)
        {
            double s=score_itr->get(class_idx).asList()->get(1).asDouble();
            class_avg[class_idx]+=s;
            if(s>max_score)
            {
                max_score=s;
                max_idx=class_idx;
            }
        }

        class_votes[max_idx]++;
    }

    double max_avg=-10000.0;
    double max_votes=-10000.0;
    int max_avg_idx;
    int max_votes_idx;
    int max_votes_sum=0;

    for(int class_idx=0; class_idx<n_classes; class_idx++)
    {
        class_avg[class_idx]=class_avg[class_idx]/n_classes;
        if(class_avg[class_idx]>max_avg)
        {
            max_avg=class_avg[class_idx];
            max_avg_idx=class_idx;
        }

        if(class_votes[class_idx]>max_votes)
        {
            max_votes=class_votes[class_idx];
            max_votes_idx=class_idx;
        }
        
        max_votes_sum+=class_votes[class_idx];
    }

    current_class=scores_buffer.front().get(max_avg_idx).asList()->get(0).asString().c_str();
    if(max_votes/scores_buffer.size()<0.2)
        current_class="?";

    cout << "Scores: " << endl;
    for (int i=0; i<n_classes; i++)
        cout << "[" << scores_buffer.front().get(i).asList()->get(0).asString().c_str() << "]: " << class_avg[i] << " "<< class_votes[i] << endl;
    cout << endl << endl;

    //plot confidence values
    if(port_out_confidence.getOutputCount()>0)
    {
        ImageOf<PixelRgb> img_conf;
        img_conf.resize(confidence_width,confidence_height);
        cvZero(img_conf.getIplImage());
        int max_height=(int)img_conf.height()*0.8;
        int min_height=img_conf.height()-20;

        int width_step=(int)img_conf.width()/n_classes;

        for(int class_idx=0; class_idx<n_classes; class_idx++)
        {
            int class_height=img_conf.height()-((int)max_height*class_votes[class_idx]/max_votes_sum);
            if(class_height>min_height)
                class_height=min_height;

            cvRectangle(img_conf.getIplImage(),cvPoint(class_idx*width_step,class_height),cvPoint((class_idx+1)*width_step,min_height),cvScalar(155,155,255),CV_FILLED);
            cvRectangle(img_conf.getIplImage(),cvPoint(class_idx*width_step,class_height),cvPoint((class_idx+1)*width_step,min_height),cvScalar(0,0,255),3);
            
//.........这里部分代码省略.........

int main(int argc, char *argv[]) {
    Network yarp;
    Port output;
	string from_port;
	string to_port;
    string fn;

	if (argc >=4) {
		from_port = string(argv[1]);
		to_port = string(argv[2]);
        fn = string(argv[3]);
	}
	else {
        if (argc < 2) {
            cout<<"usage: port_playback /thisPort /destPort filename"<<endl;
            return 0;
        }
        
	}
    
    //parse input file
    // ...
    
	output.open(from_port.c_str());

	yarp::os::Network::connect(from_port.c_str(),to_port.c_str());
	
    std::clock_t start;
	std::clock_t elapsed;

	ifstream file(fn.c_str());
	string line;
	if (file.is_open()) {
		start = std::clock();
		while (cont) {
			while (getline(file, line)) {
				cout<<"read line: "<<line<<endl;
				Bottle bot;
				bot.fromString(line.c_str());
				long t = bot.get(bot.size()-1).asInt(); //last value is timestamp
				cout<<"t="<<t<<endl;
				elapsed = std::clock() - start;
				while (elapsed <= t) {
					elapsed = std::clock() - start;
				};

				
				bot.pop(); //remove that timestamp!

				//wait, until we get past time.
				// of course, we should handle cases where there is nothing, in which case
				// it will hang!
				cout<<"sending! t="<<t<<endl;
				output.write(bot);
			}
			cont = false;
		}
    }
    output.close();
    return 0;
}

bool handPoseEstimationModule::runSMCIteration()
{
    // tmp variables
    double maxLikelihood=0.0;
    double sumLikelihood=0.0;
    double likelihood=0.0;

    // prepare containers to send data through YARP
    ImageOf<PixelBgr> &yarpReturnImage = LRimageOutputPort.prepare();
    Bottle &outputParticles = particlesOutPort.prepare();
    Bottle &outputHead= headOutPort.prepare();
    // prepare concatenated Image
    concatenatedImage.convertTo(concatenatedImage,CV_8UC3);
    yarpReturnImage.resize(concatenatedImage.cols,concatenatedImage.rows);
    concatenatedImage.copyTo( cvarrToMat( static_cast<IplImage*> ( yarpReturnImage.getIplImage() ) ) );
    // Fill Bottle with offsets+encoders to generate
    for(int index=0;index < nParticles;index++) 
    {
        // Arm + offsets
        for (unsigned int joint=0;joint<7;joint++) 
        {
            outputParticles.addDouble(encodersArm[joint]+cvmGet(particles,joint,index));
            if(index==0)
            {
                yInfo() << cvmGet(particles,joint,index);
            }
        }
        // Fingers
        for(unsigned int joint=7;joint<16;joint++) 
        {
            outputParticles.addDouble(encodersArm[joint]);
        }
    }
    outputParticles.addInt(nParticles); // n_particles

    // Fill Bottle with head encoders
    for(int k=0;k<6;k++)
    {
        outputHead.addDouble(encodersHead[k]);
    }

    //Send data to InternalModel
    particlesOutPort.write();
    headOutPort.write();  
    LRimageOutputPort.write();

    // Waitint for results
    yInfo("Waiting for Hypotheses generation and evaluation");
    Bottle *receivedLikelihood = likelihoodPort.read();
    yInfo(" DONE");
    
    // Save the likelihood of each particle
    for(int index=0;index<nParticles;index++) 
    {
        likelihood = receivedLikelihood->pop().asDouble();
        cvmSet(particles,7,index,likelihood);
        sumLikelihood+=likelihood;
        if(likelihood>maxLikelihood) 
        {
            maxLikelihood=likelihood;
        }
    }
    cvmSet(particles,7,0,0.0);
    kernelDensityEstimation();


    yInfo("Best likelihood: %f", (float) cvmGet(particles,7,maxWeightIndex));

    if(iteration>minimumIteration) // START sending offsets
    {
        // Send Best Particle
        Bottle &bestOffset = offsetsPort.prepare();
        bestOffset.clear();
        for(int i=0;i<7;i++) 
        {
            bestOffset.addDouble(cvmGet(particles,i,maxWeightIndex));
        }
        lastBestOffset.clear();
        lastBestOffset = bestOffset;
        bestOffset.addDouble(iteration);
        offsetsPort.write();
    }
    // Resampling or not Resampling. That's the Question

    if(maxLikelihood>minimumLikelihood) 
    {
        systematic_resampling(particles1to7,particles8,newParticles,cumWeight, sumLikelihood);
        artifNoiseStdDev=artifNoiseStdDev*decreasedMultiplier;
    }
    else  //I can't apply a resampling with all weights equal to 0! 
    {
        cvCopy(particles,newParticles);
        artifNoiseStdDev=artifNoiseStdDev*increasedMultiplier;
    }
    if(artifNoiseStdDev > upperBoundNoise) 
    {
       artifNoiseStdDev = upperBoundNoise;
    }
    // Apply artificial Dynamics

//.........这里部分代码省略.........