void ParticleFilter :: ResampleParticles() {
if(highestWeight == 0) { ParticleList = PropogatedList; return; }
vector<Particle> sampleSpace (NUM_PARTICLES);
for(unsigned int i = 0; i < PropogatedList.size(); i++) {
// This normalizes the weights around 1
PropogatedList[i].Weight = PropogatedList[i].Weight / highestWeight;
if(PropogatedList[i].Weight > 0.8) {
for(int j = 0; j < 5; j++)
sampleSpace.push_back(PropogatedList[i]);
}
else if(PropogatedList[i].Weight > 0.7) {
for(int j = 0; j < 4; j++)
sampleSpace.push_back(PropogatedList[i]);
}
else if(PropogatedList[i].Weight > 0.6) {
for(int j = 0; j < 3; j++)
sampleSpace.push_back(PropogatedList[i]);
}
else if(PropogatedList[i].Weight > 0.5) {
for(int j = 0; j < 2; j++)
sampleSpace.push_back(PropogatedList[i]);
}
else if(PropogatedList[i].Weight > 0.4) {
for(int j = 0; j < 1; j++)
sampleSpace.push_back(PropogatedList[i]);
}
}
for(unsigned int i = 0; i < ParticleList.size(); i++) {
ParticleList[i] = sampleSpace[rand() % sampleSpace.size()];
}
}
bool RRT::sampleFree(KDL::Frame &sample_free) const {
KDL::Frame x;
for (int i=0; i < 100; i++) {
sampleSpace(x);
if (isPoseValid(x)) {
sample_free = x;
return true;
}
}
return false;
}
