void SideConfidenceProvider::updateBallConfidences()
{
// Check, if a mirrorcle has occured:
if(thePotentialRobotPose.poseSetTime == lastFrameTime)
{
ballConfidences.init();
averageBallConfidence = UNKNOWN;
}
// Special handling for certain game states:
if(theGameInfo.state != STATE_PLAYING || theRobotInfo.penalty != PENALTY_NONE)
{
ballConfidences.init();
averageBallConfidence = UNKNOWN;
}
// Save current local ball observation:
if((theBallModel.timeWhenLastSeen == theFrameInfo.time) &&
(theFieldDimensions.isInsideField(theRobotPose * theBallModel.estimate.position)) &&
(theBallModel.estimate.velocity.abs() <= parameters.maxBallVelocity))
{
lastBallObservation = theBallModel.estimate.position;
timeOfLastBallObservation = theFrameInfo.time;
}
// Add current confidence to buffer:
if((theFrameInfo.getTimeSince(timeOfLastBallObservation) < parameters.ballBufferingInterval) &&
(theCombinedWorldModel.ballIsValidOthers) &&
(theCombinedWorldModel.ballStateOthers.velocity.abs() <= parameters.maxBallVelocity))
{
ballConfidences.add(computeCurrentBallConfidence());
timeOfLastBallObservation = 0; //For next confidence computation, a "fresh" observation is needed
}
else
{
ballConfidences.add(UNKNOWN);
}
// Check current buffer content for possible mirror situation
if(ballConfidences.getNumberOfEntries() != ballConfidences.getMaxEntries())
{
return;
}
int mirrorCount(0);
int okCount(0);
for(int i=0; i<ballConfidences.getNumberOfEntries(); ++i)
{
switch(ballConfidences[i])
{
case MIRROR: ++mirrorCount; break;
case OK: ++okCount; break;
default: ; // do nothing
}
}
int unknownCount = ballConfidences.getNumberOfEntries() - mirrorCount - okCount;
if((okCount == 0) && (3 * mirrorCount > unknownCount))
averageBallConfidence = MIRROR;
else if((3 * okCount > unknownCount) && (mirrorCount == 0))
averageBallConfidence = OK;
else
averageBallConfidence = UNKNOWN;
}
void SideConfidenceProvider::updateBallConfidences(SideConfidence& sideConfidence)
{
// Check, if a mirrorcle has occured in the last frame
if(sideConfidence.mirror)
{
confidenceBuffer.clear();
averageBallConfidence = UNKNOWN;
sideConfidence.mirror = false;
}
// Special handling for certain game states:
if(theGameInfo.state != STATE_PLAYING || theRobotInfo.penalty != PENALTY_NONE)
{
confidenceBuffer.clear();
averageBallConfidence = UNKNOWN;
}
// Save current local ball observation:
if((theBallModel.timeWhenLastSeen == theFrameInfo.time && theFrameInfo.time != 0) &&
(theFieldDimensions.isInsideField(theRobotPose * theBallModel.estimate.position)) &&
(theBallModel.estimate.velocity.norm() <= maxBallVelocity) &&
(theRobotPose * theBallModel.estimate.position).norm() > centerBanZoneRadius)
{
lastBallObservation = theBallModel.estimate.position;
timeOfLastBallObservation = theFrameInfo.time;
}
// Odometry update for buffered perception
else if(theFrameInfo.getTimeSince(timeOfLastBallObservation) < ballBufferingInterval)
{
lastBallObservation = theOdometer.odometryOffset.inverse() * lastBallObservation;
}
// Add current confidence to buffer:
if((theFrameInfo.getTimeSince(timeOfLastBallObservation) < ballBufferingInterval) &&
(theLocalizationTeamBall.isValid) &&
(theLocalizationTeamBall.position.norm() > centerBanZoneRadius) &&
(theLocalizationTeamBall.lastObservation > timeOfLastTeamBallObservation))
{
SideConfidenceMeasurement scm;
scm.ballConfidence = computeCurrentBallConfidence();
scm.timeStamp = timeOfLastBallObservation;
confidenceBuffer.push_front(scm);
timeOfLastBallObservation = 0; //For next confidence computation, a "fresh" observation is needed
timeOfLastTeamBallObservation = theLocalizationTeamBall.lastObservation;
}
else
{
return;
}
// Check current buffer content for possible mirror situation
if(!confidenceBuffer.full())
{
return;
}
size_t mirrorCount(0);
size_t okCount(0);
for(const SideConfidenceMeasurement& confidence : confidenceBuffer)
{
switch(confidence.ballConfidence)
{
case MIRROR: ++mirrorCount; break;
case OK: ++okCount; break;
}
}
size_t unknownCount = confidenceBuffer.size() - mirrorCount - okCount;
if((okCount == 0) && (mirrorCount > unknownCount))
averageBallConfidence = MIRROR;
else if((okCount > unknownCount) && (mirrorCount == 0))
averageBallConfidence = OK;
else
averageBallConfidence = UNKNOWN;
}
