void BallLocator::update(BallModel& ballModel)
{
DECLARE_DEBUG_DRAWING("module:BallLocator:Field", "drawingOnField");
DECLARE_DEBUG_DRAWING("module:BallLocator:Image", "drawingOnImage");
#ifdef TARGET_SIM
if(theFrameInfo.time <= lastFrameTime)
{
if(theFrameInfo.time < lastFrameTime)
init();
else
return;
}
#endif
// set extra members for seen ball to achieve the following behavior:
// if the ball has been seen in the last lower camera image, it will be ignored in
// the upper image to exclude any clutter in the robot's environment
ballWasSeenInThisFrame = theBallPercept.ballWasSeen;
if(ballWasSeenInThisFrame && theCameraInfo.camera == CameraInfo::upper && ballWasBeenSeenInLastLowerCameraImage)
ballWasSeenInThisFrame = false;
else if(theCameraInfo.camera == CameraInfo::lower)
ballWasBeenSeenInLastLowerCameraImage = ballWasSeenInThisFrame;
// perform prediction step for each filter
motionUpdate(ballModel);
// detect and handle collision with feet
Pose3D leftFoot = theTorsoMatrix;
Pose3D rightFoot = theTorsoMatrix;
leftFoot.conc(theRobotModel.limbs[MassCalibration::footLeft]).translate(footOffset.x, footOffset.y, -theRobotDimensions.heightLeg5Joint);
rightFoot.conc(theRobotModel.limbs[MassCalibration::footRight]).translate(footOffset.x, -footOffset.y, -theRobotDimensions.heightLeg5Joint);
Vector2<> leftFootCenter(leftFoot.translation.x, leftFoot.translation.y);
Vector2<> rightFootCenter(rightFoot.translation.x, rightFoot.translation.y);
COMPLEX_DRAWING("module:BallLocator:Image",
{
Vector2<int> leftCenterImage, leftTopImage, rightCenterImage, rightTopImage;
if(Geometry::calculatePointInImage(Vector3<>(leftFootCenter.x, leftFootCenter.y, 0), theCameraMatrix, theCameraInfo, leftCenterImage) &&
Geometry::calculatePointInImage(Vector3<>(leftFootCenter.x + footRadius, leftFootCenter.y, 0), theCameraMatrix, theCameraInfo, leftTopImage))
{
CIRCLE("module:BallLocator:Image", leftCenterImage.x, leftCenterImage.y, (leftCenterImage - leftTopImage).abs(), 0, Drawings::ps_solid, ColorRGBA(0, 0, 0), Drawings::bs_null, ColorRGBA());
}
if(Geometry::calculatePointInImage(Vector3<>(rightFootCenter.x, rightFootCenter.y, 0), theCameraMatrix, theCameraInfo, rightCenterImage) &&
Geometry::calculatePointInImage(Vector3<>(rightFootCenter.x + footRadius, rightFootCenter.y, 0), theCameraMatrix, theCameraInfo, rightTopImage))
{
CIRCLE("module:BallLocator:Image", rightCenterImage.x, rightCenterImage.y, (rightCenterImage - rightTopImage).abs(), 0, Drawings::ps_solid, ColorRGBA(0, 0, 0), Drawings::bs_null, ColorRGBA());
}
});
ENTRYPOINT void
draw_noof (ModeInfo *mi)
{
int i;
noof_configuration *bp = &bps[MI_SCREEN(mi)];
if (!bp->glx_context)
return;
glXMakeCurrent(MI_DISPLAY(mi), MI_WINDOW(mi), *(bp->glx_context));
/**
if((random() & 0xff) == 0x34){
glClear(GL_COLOR_BUFFER_BIT);
}
if((tko & 0x1f) == 0x1f){
glEnable(GL_BLEND);
glColor4f(0.0, 0.0, 0.0, 0.09);
glRectf(0.0, 0.0, wd, ht);
glDisable(GL_BLEND);
#ifdef __sgi
sginap(0);
#endif
}
*/
gravity(bp, -2.0);
for (i = 0; i < N_SHAPES; i++) {
motionUpdate(bp, i);
colorUpdate(bp, i);
drawleaf(bp, i);
}
if (mi->fps_p) do_fps (mi);
glFinish();
/* For some reason this hack screws up on Cocoa if we try to double-buffer it.
It looks fine single-buffered, so let's just do that. */
/* glXSwapBuffers(MI_DISPLAY(mi), MI_WINDOW(mi)); */
}
ENTRYPOINT void
draw_noof (ModeInfo *mi)
{
int i;
noof_configuration *bp = &bps[MI_SCREEN(mi)];
if (!bp->glx_context)
return;
glXMakeCurrent(MI_DISPLAY(mi), MI_WINDOW(mi), *(bp->glx_context));
mi->polygon_count = 0;
/**
if((random() & 0xff) == 0x34){
glClear(GL_COLOR_BUFFER_BIT);
}
if((tko & 0x1f) == 0x1f){
glEnable(GL_BLEND);
glColor4f(0.0, 0.0, 0.0, 0.09);
glRectf(0.0, 0.0, wd, ht);
glDisable(GL_BLEND);
#ifdef __sgi
sginap(0);
#endif
}
*/
gravity(bp, -2.0);
for (i = 0; i < N_SHAPES; i++) {
motionUpdate(bp, i);
colorUpdate(bp, i);
mi->polygon_count += drawleaf(bp, i);
}
if (mi->fps_p) do_fps (mi);
glFinish();
if (dbuf_p)
glXSwapBuffers(MI_DISPLAY(mi), MI_WINDOW(mi));
}
void
oneFrame(void)
{
int i;
/**
if((random() & 0xff) == 0x34){
glClear(GL_COLOR_BUFFER_BIT);
}
if((tko & 0x1f) == 0x1f){
glEnable(GL_BLEND);
glColor4f(0.0, 0.0, 0.0, 0.09);
glRectf(0.0, 0.0, wd, ht);
glDisable(GL_BLEND);
#ifdef __sgi
sginap(0);
#endif
}
*/
gravity(-2.0);
for (i = 0; i < N_SHAPES; i++) {
motionUpdate(i);
#ifdef __sgi
sginap(0);
#endif
colorUpdate(i);
#ifdef __sgi
sginap(0);
#endif
drawleaf(i);
#ifdef __sgi
sginap(0);
#endif
}
glFlush();
}
void SelfLocator::update(PotentialRobotPose& robotPose)
{
//MODIFY("module:SelfLocator:parameter", *parameter);
// recreate sampleset if number of samples was changed
ASSERT(samples);
if(parameter->numberOfSamples != samples->size())
init();
// Maybe pose has already been computed by call to other update function (for clusters)
if(lastPoseComputationTimeStamp == bb->theFrameInfo.time)
{
robotPose = (PotentialRobotPose&)lastComputedPose;
return;
}
// Normal computation:
preExecution(robotPose);
bool templatesOnly(false);
bool odometryOnly(false);
//DEBUG_RESPONSE("module:SelfLocator:templates only", templatesOnly = true;);
//DEBUG_RESPONSE("module:SelfLocator:odometry only", odometryOnly = true;);
if(templatesOnly) //debug
{
if(sampleTemplateGenerator.templatesAvailable())
for(int i = 0; i < this->samples->size(); ++i)
generateTemplate(samples->at(i));
poseCalculator->calcPose(robotPose);
}
else if(odometryOnly) //debug
{
robotPose += bb->theOdometryData - lastOdometry;
lastOdometry = bb->theOdometryData;
}
else //normal case
{
motionUpdate(updatedBySensors);
updatedBySensors = applySensorModels();
if(updatedBySensors)
{
adaptWeightings();
resampling();
}
poseCalculator->calcPose(robotPose);
}
robotPose.deviation = 100000.f;
lastComputedPose = robotPose;
lastPoseComputationTimeStamp = bb->theFrameInfo.time;
// drawings
/*DECLARE_DEBUG_DRAWING("module:SelfLocator:poseCalculator", "drawingOnField"); // Draws the internal representations for pose computation
COMPLEX_DRAWING("module:SelfLocator:poseCalculator", poseCalculator->draw());
DECLARE_DEBUG_DRAWING("module:SelfLocator:perceptValidityChecker", "drawingOnField"); // Draws the internal representations for percept validity checking
COMPLEX_DRAWING("module:SelfLocator:perceptValidityChecker", perceptValidityChecker->draw());
DECLARE_DEBUG_DRAWING("module:SelfLocator:samples", "drawingOnField"); // Draws the internal representations of the goal locator
COMPLEX_DRAWING("module:SelfLocator:samples", drawSamples());
DECLARE_DEBUG_DRAWING("module:SelfLocator:templates", "drawingOnField"); // Draws all available templates
COMPLEX_DRAWING("module:SelfLocator:templates", sampleTemplateGenerator.draw(););
DECLARE_DEBUG_DRAWING("module:SelfLocator:selectedPoints", "drawingOnField");*/
//fieldModel.draw();
}
void ParticleFilterBallLocator::execute()
{
getBallModel().reset();
// update by motion model
motionUpdate(theSampleSet, true);
if(getBallPercept().ballWasSeen)
{
if(theSampleSet.size() > 0)
{
//
updateByBallPercept(theSampleSet);
//
resampleGT07(theSampleSet, false);
}//end if
// replace a random particle
if(perceptBuffer.isFull())
{
if(theSampleSet.size() < 20)
{
theSampleSet.push_back(generateNewSample());
}
else
{
int k = Math::random(theSampleSet.size());
theSampleSet[k] = generateNewSample();
}
}
}//end if
// calculate the model
if(!theSampleSet.empty())
{
Vector2<double> mean;
Vector2<double> meanSpeed;
double numberOfMovingSamples = 0;
for (unsigned int i = 0; i < theSampleSet.size(); i++)
{
mean += theSampleSet[i].position;
if(theSampleSet[i].moving)
{
meanSpeed += theSampleSet[i].speed;
numberOfMovingSamples++;
}
}//end for
double meanSpeedAbs = meanSpeed.abs();
MODIFY("ParticleFilterBallLocator:meanSpeadAbs", meanSpeedAbs);
MODIFY("ParticleFilterBallLocator:numberOfMovingSamples", numberOfMovingSamples);
mean /= theSampleSet.size();
if(numberOfMovingSamples > 1)
meanSpeed /= numberOfMovingSamples;
// set the ball model
if(getBallPercept().ballWasSeen)
getBallModel().setFrameInfoWhenBallWasSeen(getFrameInfo());
getBallModel().position = mean;
//TODO change 13.03
//getBallModel().speed = Vector2<double>();
getBallModel().speed = meanSpeed*100;
getBallModel().valid = true;
updatePreviewModel();
}//end if
DEBUG_REQUEST("ParticleFilterBallLocator:draw_ball_on_field", drawBallModel(getBallModel()); );
