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; }