void ParticleFilter::resetLocToSide(bool blueSide)
{
// Clear the existing particles.
particles.clear();
float xLowerBound = 0.0f, xUpperBound = 0.0f;
// HACK replace constants!
float yLowerBound = FIELD_WHITE_BOTTOM_SIDELINE_Y, yUpperBound = FIELD_WHITE_TOP_SIDELINE_Y;
float heading = 0.0f;
boost::mt19937 rng;
rng.seed(std::time(0));
if(blueSide)
{
xLowerBound = FIELD_WHITE_LEFT_SIDELINE_X;
xUpperBound = MIDFIELD_X;
}
else
{
xLowerBound = MIDFIELD_X;
xUpperBound = FIELD_WHITE_RIGHT_SIDELINE_X;
heading = M_PI_FLOAT;
}
boost::uniform_real<float> xBounds(xLowerBound,
xUpperBound);
boost::uniform_real<float> yBounds(yLowerBound,
yUpperBound);
boost::variate_generator<boost::mt19937&,
boost::uniform_real<float> > xGen(rng, xBounds);
boost::variate_generator<boost::mt19937&,
boost::uniform_real<float> > yGen(rng, yBounds);
// Assign uniform weight.
float weight = 1.0f/(((float)parameters.numParticles)*1.0f);
for(int i = 0; i < parameters.numParticles; ++i)
{
Particle p(xGen(), yGen(), heading, weight);
particles.push_back(p);
}
}
ParticleFilter::ParticleFilter(ParticleFilterParams params)
: parameters(params),
estimateUncertainty(3, 0.0f)
{
motionSystem = new MotionSystem(params.odometryXYNoise,
params.odometryHNoise);
visionSystem = new VisionSystem;
boost::mt19937 rng;
rng.seed(std::time(0));
boost::uniform_real<float> xBounds(0.0f,
(float) parameters.fieldWidth);
boost::uniform_real<float> yBounds(0.0f,
(float) parameters.fieldHeight);
boost::uniform_real<float> angleBounds(0,
2.0f*boost::math::constants::pi<float>());
boost::variate_generator<boost::mt19937&,
boost::uniform_real<float> > xGen(rng, xBounds);
boost::variate_generator<boost::mt19937&,
boost::uniform_real<float> > yGen(rng, yBounds);
boost::variate_generator<boost::mt19937&,
boost::uniform_real<float> > angleGen(rng, angleBounds);
// Assign uniform weight.
float weight = 1.0f/(((float)parameters.numParticles)*1.0f);
for(int i = 0; i < parameters.numParticles; ++i)
{
messages::RobotLocation randomLocation;
randomLocation.set_x(xGen());
randomLocation.set_y(yGen());
randomLocation.set_h(angleGen());
Particle p(randomLocation, weight);
particles.push_back(p);
}
lost = false;
}
bool RenderThemeWinCE::paintSearchFieldCancelButton(RenderObject* o, const PaintInfo& paintInfo, const IntRect& r)
{
Color buttonColor = (o->node() && o->node()->active()) ? Color(138, 138, 138) : Color(186, 186, 186);
IntSize cancelSize(10, 10);
IntSize cancelRadius(cancelSize.width() / 2, cancelSize.height() / 2);
int x = r.x() + (r.width() - cancelSize.width()) / 2;
int y = r.y() + (r.height() - cancelSize.height()) / 2 + 1;
IntRect cancelBounds(IntPoint(x, y), cancelSize);
paintInfo.context->save();
paintInfo.context->addRoundedRectClip(RoundedRect(cancelBounds, cancelRadius, cancelRadius, cancelRadius, cancelRadius));
paintInfo.context->fillRect(cancelBounds, buttonColor, ColorSpaceDeviceRGB);
// Draw the 'x'
IntSize xSize(3, 3);
IntRect xBounds(cancelBounds.location() + IntSize(3, 3), xSize);
paintInfo.context->setStrokeColor(Color::white, ColorSpaceDeviceRGB);
paintInfo.context->drawLine(xBounds.location(), xBounds.location() + xBounds.size());
paintInfo.context->drawLine(IntPoint(xBounds.maxX(), xBounds.y()), IntPoint(xBounds.x(), xBounds.maxY()));
paintInfo.context->restore();
return false;
}
/*
* @brief The following are all of the resetLoc functions
*/
void ParticleFilter::resetLoc()
{
// Clear the existing particles.
particles.clear();
boost::mt19937 rng;
rng.seed(std::time(0));
boost::uniform_real<float> xBounds(0.0f,
(float) parameters.fieldWidth);
boost::uniform_real<float> yBounds(0.0f,
(float) parameters.fieldHeight);
boost::uniform_real<float> angleBounds(0,
2.0f*boost::math::constants::pi<float>());
boost::variate_generator<boost::mt19937&,
boost::uniform_real<float> > xGen(rng, xBounds);
boost::variate_generator<boost::mt19937&,
boost::uniform_real<float> > yGen(rng, yBounds);
boost::variate_generator<boost::mt19937&,
boost::uniform_real<float> > angleGen(rng, angleBounds);
// Assign uniform weight.
float weight = 1.0f/(((float)parameters.numParticles)*1.0f);
for(int i = 0; i < parameters.numParticles; ++i)
{
messages::RobotLocation randomLocation;
randomLocation.set_x(xGen());
randomLocation.set_y(yGen());
randomLocation.set_h(angleGen());
Particle p(randomLocation, weight);
particles.push_back(p);
}
}
