Location LocalizationManager::GetGridBestLocation()
{
Location bestLocation = GetBestParticle().getLocation();
Point gridPoint = _map->getGridPointBy(bestLocation.GetPoint());
return Location(gridPoint, bestLocation.GetYawPoint());
}
LocalizationManager::LocalizationManager(Location robotLocation, Map* map)
{
_map = map;
//create first particle by robot location
Particle first(robotLocation.GetPoint().GetX(), robotLocation.GetPoint().GetY(), robotLocation.GetYawPoint(), _map);
_particles.push_back(first);
// Create all other particles. Each time we create the particle
// as the son of the last one we created.
/*for(int i = 1; i < 100; i++)
{
Particle current = first.genereateNewParticle();
_particles.push_back(current);
}*/
//PrintParticles();
}
void LocalizationManager::Update(Location deltaLocation, float* laserScans)
{
_particles[0].Update
(
deltaLocation.GetPoint().GetX(),
deltaLocation.GetPoint().GetY(),
deltaLocation.GetYawPoint(),
laserScans
);
/*double currentBelief;
for(unsigned int i = 0; i < _particles.size(); i ++)
{
currentBelief = _particles[i].Update
(
deltaLocation.GetPoint().GetX(),
deltaLocation.GetPoint().GetY(),
deltaLocation.GetYawPoint(),
laserScans
);
if(currentBelief < lowerThreshold )
{
_particles.erase(_particles.begin() + i);
}
else if ((currentBelief > upperThreshold) && _particles.size() < 100)
{
Particle newChild = _particles[i].genereateNewParticle();
_particles.push_back(newChild);
}
}
//PrintParticles();*/
}