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;
// cout << neuronData.size() << endl;
Bottle &neuronData = testHandNeuronsPort.prepare();
neuronData.clear();
//neuronData = leftHandNeuronsPort.prepare();
neuronData.addInt(i-xMin);
neuronData.addInt(j-yMin);
neuronData.addInt(spikeTime);
Bottle &velocityData = leftHandVelocityPort.prepare();
velocityData.addDouble(Xhat);
velocityData.addDouble(Yhat);
velocityData.addDouble(Zhat);
leftHandVelocityPort.write();
// cout << neuronData.toString().c_str() << endl;
if( spikeTime >0)// && (t-spikeTime)<10)
{
testHandNeuronsPort.write();
}
Time::delay(0.001);
//cout << leftHandNeuronsPort.getPendingReads() << endl;
neuronData.clear();
velocityData.clear();
}
}
//Bottle *testData = testHandNeuronsPort.read();
//cout << (*testData).size() << endl;
//t = t+1;
Xp = X; Yp=Y;Zp=Z;
}
}
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);
CvFont font;
cvInitFont(&font,CV_FONT_HERSHEY_SIMPLEX,0.6,0.6,0,2);
cvPutText(img_conf.getIplImage(),scores_buffer.front().get(class_idx).asList()->get(0).asString().c_str(),cvPoint(class_idx*width_step,img_conf.height()-5),&font,cvScalar(255,255,255));
}
port_out_confidence.write(img_conf);
}
mutex.post();
}
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
CvMat* A = cvCreateMat(8,8,CV_32FC1);
cvSetIdentity(A); //
cvMatMul(A,newParticles,particles);
float mean = 0;
CvMat* noiseSingle;
noiseSingle = cvCreateMat(1,nParticles,CV_32FC1);
cvSetZero(noiseSingle);
if(artifNoiseStdDev < lowerBoundNoise) // lowerbound of artificial noise
{
artifNoiseStdDev = lowerBoundNoise;
}
cvRandArr( &rngState, noise, CV_RAND_NORMAL, cvScalar(mean), cvScalar(artifNoiseStdDev));
cvAdd(particles1to7,noise,particles1to7);
yInfo() << "ArtNoise: " << artifNoiseStdDev;
// Setting first particle as Zero offset
for (unsigned int joint=0;joint<8;joint++)
{
cvmSet(particles,joint,0,0.0);
}
return true;
}
