SideConfidenceProvider::BallModelSideConfidence SideConfidenceProvider::computeCurrentBallConfidence()
{
// Some constant parameters
const float distanceObserved = lastBallObservation.norm();
const float angleObserved = lastBallObservation.angle();
const float& camZ = theCameraMatrix.translation.z();
const float distanceAsAngleObserved = (pi_2 - std::atan2(camZ,distanceObserved));
// Weighting for original pose
float originalWeighting = computeAngleWeighting(angleObserved, theLocalizationTeamBall.position, theRobotPose,
standardDeviationBallAngle);
originalWeighting *= computeDistanceWeighting(distanceAsAngleObserved, theLocalizationTeamBall.position, theRobotPose,
camZ, standardDeviationBallDistance);
// Weighting for mirrored pose
const Pose2f mirroredPose = Pose2f(pi) + (Pose2f)(theRobotPose);
float mirroredWeighting = computeAngleWeighting(angleObserved, theLocalizationTeamBall.position, mirroredPose,
standardDeviationBallAngle);
mirroredWeighting *= computeDistanceWeighting(distanceAsAngleObserved, theLocalizationTeamBall.position, mirroredPose,
camZ, standardDeviationBallDistance);
PLOT("module:SideConfidenceProvider:originalWeighting", originalWeighting);
PLOT("module:SideConfidenceProvider:mirroredWeighting", mirroredWeighting);
// Decide state based on weights
if((mirroredWeighting < minWeighting) && (originalWeighting < minWeighting))
return UNKNOWN;
if((mirroredWeighting > weightingFactor * originalWeighting) || (originalWeighting > weightingFactor * mirroredWeighting))
return mirroredWeighting > originalWeighting ? MIRROR : OK;
return UNKNOWN;
}
SideConfidenceProvider::BallModelSideConfidence SideConfidenceProvider::computeCurrentBallConfidence()
{
// Some constant parameters
const float distanceObserved = lastBallObservation.abs();
const float angleObserved = lastBallObservation.angle();
const float& camZ = theCameraMatrix.translation.z;
const float distanceAsAngleObserved = (pi_2 - atan2(camZ,distanceObserved));
// Weighting for original pose
float originalWeighting = computeAngleWeighting(angleObserved, theCombinedWorldModel.ballStateOthers.position, theRobotPose,
parameters.standardDeviationBallAngle);
originalWeighting *= computeDistanceWeighting(distanceAsAngleObserved, theCombinedWorldModel.ballStateOthers.position, theRobotPose,
camZ, parameters.standardDeviationBallDistance);
// Weighting for mirrored pose
const Pose2D mirroredPose = Pose2D(pi) + (Pose2D)(theRobotPose);
float mirroredWeighting = computeAngleWeighting(angleObserved, theCombinedWorldModel.ballStateOthers.position, mirroredPose,
parameters.standardDeviationBallAngle);
mirroredWeighting *= computeDistanceWeighting(distanceAsAngleObserved, theCombinedWorldModel.ballStateOthers.position, mirroredPose,
camZ, parameters.standardDeviationBallDistance);
// Decide state based on weights
if((mirroredWeighting > parameters.weightingFactor * originalWeighting) || (originalWeighting > parameters.weightingFactor * mirroredWeighting))
return mirroredWeighting > originalWeighting ? MIRROR : OK;
return UNKNOWN;
}
