void VectorLocalization2D::initialize(int _numParticles, const char* mapName, vector2f loc, float angle, float locationUncertainty, float angleUncertainty)
{
static const bool debug = false;
currentMap = &maps[0];
bool found = false;
for(unsigned int i=0; i<maps.size(); i++){
if(maps[i].mapName.compare(mapName)==0){
found = true;
currentMap = &maps[i];
}
}
if(!found){
char buf[2048];
snprintf(buf,2047,"Map %s not found in Atlas! Reverting to map %s.",mapName,maps[0].mapName.c_str());
TerminalWarning(buf);
}
numParticles = _numParticles;
particles.resize(numParticles);
if(debug) printf(" Initializing particles: 0.0%%\r");
fflush(stdout);
for(int i=0; i<numParticles; i++){
particles[i] = Particle2D(DBL_MAX,DBL_MAX,0.0,0.0);
}
for(int i=0; i<numParticles; i++){
if(debug && (i%10)==0){
printf(" Initializing particles: %5.1f%%\r",float(i)/float(numParticles)*100.0);
fflush(stdout);
}
particles[i] = createParticle(loc, angle, locationUncertainty, angleUncertainty);
}
computeLocation(loc, angle);
laserEval.numCorrespondences = 0;
laserEval.numObservedPoints = 0;
laserEval.runTime = 0;
laserEval.stage0Weights = 0;
laserEval.stageRWeights = 0;
pointCloudEval.numCorrespondences = 0;
pointCloudEval.numObservedPoints = 0;
pointCloudEval.runTime = 0;
pointCloudEval.stage0Weights = 0;
pointCloudEval.stageRWeights = 0;
if(debug) printf("\nDone.\n");
}
inline void AddTexCoord(const float X, const float Y)
{
texCoord.push_back(Particle2D(X, Y));
}
Particle2D VectorLocalization2D::createParticle(vector2f loc, float angle, float locationUncertainty, float angleUncertainty)
{
return Particle2D(randn(locationUncertainty,loc.x), randn(locationUncertainty,loc.y), randn(angleUncertainty,angle),1.0);
}
