/**
* returns the state vector for the best particle as RigidBodyState
*/
virtual base::samples::RigidBodyState& estimate() {
base::Matrix3d variance = base::Matrix3d::Zero();
ParticleIterator best = particles.begin();
//Only calculate a new estimate, if we have a new observation or update
if(changed_particles){
for(ParticleIterator it = particles.begin(); it != particles.end(); ++it) {
base::Vector3d pos_s = position(*it) - mean_position;
variance += pos_s * pos_s.transpose();
if( confidence(*best) < confidence(*it) )
best = it;
}
state = orientation(*best);
state.position = position(*best);
state.cov_position = variance / (particles.size() + 1);
}
else{
state = orientation(*best);
state.position.x() = last_pose.x();
state.position.y() = last_pose.y();
state.cov_position = last_cov;
}
changed_particles = false;
last_pose = state.position;
last_cov = state.cov_position;
return state;
}
double observe(const Z& z, M& m, double ratio = 1.0)
{
Perception<P, Z, M>* model = dynamic_cast<Perception<P, Z, M>*>(this);
std::vector<double> perception_weights;
perception_weights.reserve(particles.size());
double sum_perception_weight = 0.0;
double sum_main_confidence = 0.0;
double Neff = 0.0;
unsigned i;
changed_particles = true;
// calculate all perceptions
for(ParticleIterator it = particles.begin(); it != particles.end(); it++) {
perception_weights.push_back(model->perception(*it, z, m));
sum_perception_weight += perception_weights.back();
}
if(sum_perception_weight <= 0.0)
return 0.0;
i = 0;
// normalize perception weights and form mixed weight based on current weight.
for(ParticleIterator it = particles.begin(); it != particles.end(); it++) {
double uniform = 1.0 / particles.size();
double w = 1.0 - ratio;
double main_confidence = confidence(*it) * ((ratio * (perception_weights[i] / sum_perception_weight))
+ w * uniform);
setConfidence(*it, main_confidence);
sum_main_confidence += main_confidence;
i++;
}
// normalize overall confidence
for(ParticleIterator it = particles.begin(); it != particles.end(); it++) {
double weight;
if(sum_main_confidence == 0.0)
weight = 0.0;
else
weight = confidence(*it) / sum_main_confidence;
setConfidence(*it, weight);
Neff += weight * weight;
}
if(Neff == 0.0)
effective_sample_size = 1.0;
else
effective_sample_size = (1.0 / Neff) / particles.size();
return effective_sample_size;
}
/**
* return the state for the weighted avegrage particle as RBS
*/
virtual base::samples::RigidBodyState& estimate_middle() {
base::Matrix3d variance = base::Matrix3d::Zero();
ParticleIterator best = particles.begin();
base::Vector3d best_pos(0.0, 0.0, 0.0);
double sum_conf = 0.0;
//Estimate a new position, only if we had an update or an observation!
if(changed_particles){
for(ParticleIterator it = particles.begin(); it != particles.end(); ++it) {
if(!isValid(*it))
continue;
base::Vector3d pos_s = position(*it) - mean_position;
variance += pos_s * pos_s.transpose();
sum_conf += confidence(*it);
best_pos += confidence(*it) * position(*it);
}
state = orientation(*best);
if(sum_conf > 0){
Eigen::Vector3d pos = best_pos / sum_conf;
state.position.x() = pos.x();
state.position.y() = pos.y();
state.cov_position = variance / (particles.size() + 1);
}
else{
state.position.x() = 0.0;
state.position.y() = 0.0;
state.cov_position = base::Matrix3d::Identity() * INFINITY;
}
}else{
state = orientation(*best);
state.position.x() = last_pose.x();
state.position.y() = last_pose.y();
state.cov_position = last_cov;
}
state.cov_velocity = base::Matrix3d::Identity();
state.cov_orientation = base::Matrix3d::Identity();
state.cov_angular_velocity = base::Matrix3d::Identity();
last_pose = state.position;
last_cov = state.cov_position;
changed_particles = false;
return state;
}
// ----------------------------------------------------------------------
bool
LocalizationNeighborInfo::
is_confident( void )
const throw()
{
return confidence() != 0 && !is_twin() && has_pos() && has_distance();
}
void SSLVision::parse(SSL_DetectionFrame &pck)
{
// update camera fps
int cid = pck.camera_id();
if(cid == 0) _fpscam0.Pulse();
if(cid == 1) _fpscam1.Pulse();
switch(_camera)
{
case CAMERA_BOTH:
break;
case CAMERA_0:
if (cid==1) return;
break;
case CAMERA_1:
if (cid==0) return;
break;
case CAMERA_NONE:
default:
return;
}
// update vision frame
//_vframe[cid].frame_number = pck.frame_number();
vector<Position> pt;
// Team side Coefficient
float ourSide = (_side == SIDE_RIGHT)? -1.0f : 1.0f;
double time = _time.elapsed(); //pck.t_capture();
// insert balls
int max_balls=min(VOBJ_MAX_NUM, pck.balls_size());
for(int i=0; i<max_balls; ++i)
{
auto b = pck.balls(i);
if(b.has_confidence() && b.has_x() && b.has_y())
if(b.confidence() > MIN_CONF && fabs(b.x()) < FIELD_MAX_X && fabs(b.y()) < FIELD_MAX_Y)
{
Position tp;
tp.loc = Vector2D(b.x()*ourSide, b.y()*ourSide);
pt.push_back(tp);
}
}
_wm->ball.seenAt(pt, time, cid);
if(_color == COLOR_BLUE)
{
APPEND_ROBOTS(blue, our);
APPEND_ROBOTS(yellow, opp);
}
else // _color == COLOR_YELLOW
{
APPEND_ROBOTS(yellow, our);
APPEND_ROBOTS(blue, opp);
}
}
/**
* normalize the weights for all particles
*/
virtual void normalizeParticles() {
double sum = 0.0;
double neff = 0.0;
base::Position mean_pos = base::Position::Zero();
for(ParticleIterator it = particles.begin(); it != particles.end(); ++it) {
mean_pos += position(*it);
sum += confidence(*it);
}
if(sum == 0.0 && particles.size() != 0){
sum = particles.size();
for(ParticleIterator it = particles.begin(); it != particles.end(); ++it) {
setConfidence(*it, 0.0);
neff += confidence(*it) * confidence(*it);
}
}
else{
for(ParticleIterator it = particles.begin(); it != particles.end(); ++it) {
setConfidence(*it, confidence(*it) / sum);
neff += confidence(*it) * confidence(*it);
}
}
mean_position = mean_pos / particles.size();
effective_sample_size = (1.0 / neff) / particles.size();
changed_particles = true;
}
/**
* execute an importance resampling process on the current particle set
* with an low variance sampler introduced by Thrun, Burgard and Fox
*/
void resample() {
if(particles.size() < 1)
return;
double m_inv = 1.0 / particles.size();
machine_learning::UniformRealRandom random = machine_learning::Random::uniform_real(0.0, m_inv);
std::list<P> set;
double r = random();
double c = confidence(particles.front());
ParticleIterator it = particles.begin();
for(unsigned i = 0, m = 0; m < particles.size(); m++) {
double u = r + (m * m_inv);
while(u > c) {
it++;
if(++i >= particles.size()) {
it = particles.begin();
i = 0;
}
c += confidence(*it);
}
set.push_back(*it);
}
particles = set;
normalizeParticles();
generation++;
changed_particles = true;
}
void
DataValidator::print()
{
if (_time_last == 0) {
ECL_INFO("\tno data");
return;
}
for (unsigned i = 0; i < dimensions; i++) {
ECL_INFO("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f conf: %8.4f",
(double) _value[i], (double)_lp[i], (double)_mean[i],
(double)_rms[i], (double)confidence(hrt_absolute_time()));
}
}
/**
* returns current status of particle set in a general form
*
* \return filled particle vector
*/
const ParticleSet& getParticleSet( base::Vector3d transformation = base::Vector3d::Zero()) {
assert(particles.size() > 0);
ps.particles.clear();
ps.timestamp = timestamp;
for(ParticleIterator it = particles.begin(); it != particles.end(); it++) {
Particle p;
p.position = position(*it) + transformation;
p.velocity = velocity(*it);
p.yaw = base::getYaw(orientation(*it).orientation);
p.main_confidence = confidence(*it);
ps.particles.push_back(p);
}
return ps;
}
double observe_markov(const Z& z, M& m, double ratio = 1.0)
{
Perception<P, Z, M>* model = dynamic_cast<Perception<P, Z, M>*>(this);
double Neff = 0.0;
double weight;
double sum = 0.0;
// calculate all perceptions
for(ParticleIterator it = particles.begin(); it != particles.end(); it++) {
double conf = model->perception(*it, z, m);
setConfidence(*it, conf);
sum += conf;
}
// normalize overall confidence
for(ParticleIterator it = particles.begin(); it != particles.end(); it++) {
if(sum == 0.0)
weight = 1 / particles.size();
else
weight = confidence(*it) / sum;
setConfidence(*it, weight);
Neff += weight * weight;
}
if(Neff == 0.0)
effective_sample_size = 1.0;
else
effective_sample_size = (1.0 / Neff) / particles.size();
changed_particles = true;
return effective_sample_size;
}
QVector<double> analyse::getWeightByFlat(double **data, int nb_valeur, Flat *indexArray, Stat *statArray) {
QVector<QVector<double> >meanValues;
QVector<double> confidence(4,0.0), res(2);
double moy=0,var=0,d,f,quar[4];
int nbR=0;
indexArray->indexStart.resize(4);
indexArray->indexStop.resize(4);
detectAllFlat(data, nb_valeur, indexArray);
meanValues = meanValuesEachFlat(data,indexArray);
lengthGoodFlats(meanValues,indexArray);
meanVarianceValuesByFlat(data,statArray,indexArray);
stat.moustach(data[3],nb_valeur,&d,&f,&moy,&var,&nbR,quar);
confidence = getTruthfulIndex(statArray->length,nbR);
double coeff1 = confidence[3]/4, coeff2, coeff3 = 0;
if( var < 0.02) {
coeff2 = 0.6-0.1*(var/0.02);
}
else if (var < 0.04) {
coeff2 = 0.5-0.2*((var-0.02)/0.02);
}
else if (var < 0.06) {
coeff2 = 0.3-0.2*((var-0.04)/0.02);
}
else {
coeff2 = 0.006/var;
}
if(nbR > 400 && nbR < 700) {
coeff3 = 0.05;
}
else if (nbR >= 700){
coeff3 = 0.15;
}
//Confidence of a Flat
// double indexFlat = 0;
// if ( statArray->var[3] > 0 ) {
// indexFlat = confidence[3] / statArray->var[3];
// }
//Confidence of a Curve
//double indexCurve = double(nb_valeur)/1000;
//Confidence of Weight
res[1] = 100*(coeff1+coeff2+coeff3);
//Weight
res[0] = statArray->sum[3];
if ( res[1] > 0.1 && isGoodFlat(res,data,nb_valeur)) {
qDebug() << "plat" << coeff1 << coeff2 << coeff3 << res[1] << var;
}
return res;
}
void run_test_time(void)
{
int i,rc;
int exit_rc = 0;
int iterations = 0;
unsigned long long x;
unsigned long long y, prev_y, total_yields = 0;
struct sembuf mysembuf1;
char pbuf[100]="1";
int regsetsize;
mysembuf1.sem_num = 0;
mysembuf1.sem_op = num_children;
mysembuf1.sem_flg = 0;
/* post the sema4 to allow children to start */
rc = semop (start_sem, &mysembuf1, 1);
if (rc == -1) {
stop_test = 1;
exit_rc = errno;
perror ("parent semop(start_sem) failed ");
goto exit_test;
}
regsetsize = num_children/num_active ;
/* start off by writing to one child in each active set */
for (i=0; !isextra(i) && i<num_children ; i+=regsetsize) {
/* printf("PARENT : waking %d initially\n",i); */
write(childpipe[i][1],pbuf,TOKENSZ);
}
/* Shortcircuit
sleep(num_seconds);
stop_test = 1 ;
return ;
*/
restart:
if (!fquiet) printf (".");
prev_y = 0;
y = 0;
/* get the start time */
rc = gettimeofday (&tv1, &tz1);
if (rc) {
stop_test = 1;
exit_rc = errno;
perror ("gettimeofday failed on tv1 ");
goto exit_test;
}
/* wait until timeout */
for (i = 0 ; i < num_children ; i++) prev_y += total[i].count;
sleep (num_seconds);
for (i = 0 ; i < num_children ; i++) y += total[i].count;
// printf("Rerun : Across children : Avg %15.2f \t Var %15.2f\n",child_avg(),child_var());
/* get end time */
rc = gettimeofday (&tv2, &tz2);
if (rc) {
stop_test = 1;
exit_rc = rc;
perror ("gettimeofday failed on tv2 ");
goto exit_test;
}
total_yields += y;
total_yields -= prev_y;
/* compute microseconds per yield */
timersub(&tv2, &tv1, &tvr); /* tvr now contains result of tv2-tv1 */
x = (unsigned long long)tvr.tv_sec * 1000000;
x+= (unsigned long long)tvr.tv_usec;
results[iterations].data = (float)x;
results[iterations].data /= (float)(y - prev_y);
iterations++;
if (confidence(iterations)) {
stop_test = 1;
} else {
goto restart;
}
if (!fquiet) printf (" Test Completed.\n");
switch (foutput) {
case 1:
printf (" microsec ave variance confidence\n");
printf ("----------- ----------- ----------- -----------\n");
for (i = 0 ; i < iterations ; i++) {
printf ("%11.3f %11.3f %11.3f %11.3f ",
results[i].data,
results[i].ave,
results[i].var,
results[i].conf);
if (i < NUM_WARMUP)
printf ("warmup");
printf ("\n");
}
break;
case 2:
if (!valid_test) break;
printf ("%u , %u , %0.3f\n", num_children, num_seconds, results[iterations-1].ave);
break;
default:
if (!valid_test) break;
printf ("%u children did %Lu rounds at %0.3f microseconds/round\n", num_children, total_yields, results[iterations-1].ave);
break;
}
exit_test:
return;
}
void cSemaineWordSender::sendKeywords( cComponentMessage *_msg )
{
int i;
juliusResult *k = (juliusResult *)(_msg->custData);
if (k==NULL) return;
int nW = 0;
for (i=0; i<k->numW; i++) {
// check for non-verbals.... and remove them, only preceed if words are left
if (k->word[i][0] != '*') nW++;
}
if (nW == 0) return;
char strtmp[150];
sprintf(strtmp,"%.2f",_msg->floatData[0]);
std::string valStr(strtmp);
long long startTime = smileTimeToSemaineTime(_msg->userTime1);
sprintf(strtmp,"%ld",startTime);
std::string startTm(strtmp);
sprintf(strtmp,"%ld",(long long)round((_msg->userTime2 - _msg->userTime1)*1000.0));
std::string duration(strtmp);
// Create and fill a simple EMMA document
XERCESC_NS::DOMDocument * document = XMLTool::newDocument(EMMA::E_EMMA, EMMA::namespaceURI, EMMA::version);
XMLTool::setPrefix(document->getDocumentElement(), "emma");
XERCESC_NS::DOMElement * sequence = XMLTool::appendChildElement(document->getDocumentElement(), EMMA::E_SEQUENCE);
XMLTool::setAttribute(sequence, EMMA::A_OFFSET_TO_START, startTm);
XMLTool::setAttribute(sequence, EMMA::A_DURATION, duration);
XMLTool::setPrefix(sequence, "emma");
for (i=0; i<k->numW; i++) {
// split combined keywords (TALK_TO_POPPY) etc. at the special character "_" and put them in individual tags
/*
char * tr = strdup(k->word[i]);
char * tmp = tr;
char * x = NULL;
do {
x = (char *)strchr(tmp, '_');
// separate at '_'
if (x!=NULL) {
*x = 0;
}
// remove spaces
//while (*tmp==' ') { tmp++; }
//size_t l = strlen(tmp);
//while (tmp[l-1] == ' ') { tmp[l-1] = 0; l--; }
// append an xml keyword tag
XERCESC_NS::DOMElement * interpretation = XMLTool::appendChildElement(sequence, EMMA::E_INTERPRETATION);
sprintf(strtmp,"%ld",startTime + (long long)round((k->start[i])*1000.0));
std::string offs(strtmp);
sprintf(strtmp,"%s",tmp);
std::string keyword(strtmp);
sprintf(strtmp,"%.3f",k->conf[i]);
std::string confidence(strtmp);
XMLTool::setAttribute(interpretation, EMMA::A_OFFSET_TO_START, offs);
XMLTool::setAttribute(interpretation, EMMA::A_TOKENS, keyword);
XMLTool::setAttribute(interpretation, EMMA::A_CONFIDENCE, confidence);
XMLTool::setPrefix(interpretation, "emma");
// analyse next part of string, if present
if (x != NULL) {
tmp = x+1;
}
} while (x != NULL);
free(tr);
*/
// one word:
if (k->word[i][0] != '*') {
XERCESC_NS::DOMElement * interpretation = XMLTool::appendChildElement(sequence, EMMA::E_INTERPRETATION);
sprintf(strtmp,"%ld",startTime + (long long)round((k->start[i])*1000.0));
std::string offs(strtmp);
sprintf(strtmp,"%s",k->word[i]);
std::string keyword(strtmp);
sprintf(strtmp,"%.3f",k->conf[i]);
std::string confidence(strtmp);
XMLTool::setAttribute(interpretation, EMMA::A_OFFSET_TO_START, offs);
XMLTool::setAttribute(interpretation, EMMA::A_TOKENS, keyword);
XMLTool::setAttribute(interpretation, EMMA::A_CONFIDENCE, confidence);
XMLTool::setPrefix(interpretation, "emma");
}
}
// Now send it
sendDocument(document);
}
double Context::negative_confidence(const char* intent) const{
return confidence(false, intent);
}
double Context::positive_confidence(const char* intent) const{
return confidence(true, intent);
}
