bool BallPerceptor::checkBallSpot(const BallSpot& ballSpot, const ImageCoordinateSystem& theImageCoordinateSystem,
const CameraInfo& theCameraInfo,
const CameraMatrix& theCameraMatrix,
const FieldDimensions& theFieldDimensions)
{
// calculate an approximation of the radius based on bearing distance of the ball spot
const Vector2<int>& spot = ballSpot.position;
Vector2<> correctedStart = theImageCoordinateSystem.toCorrected(spot);
Vector3<> cameraToStart(theCameraInfo.focalLength, theCameraInfo.opticalCenter.x - correctedStart.x, theCameraInfo.opticalCenter.y - correctedStart.y);
Vector3<> unscaledField = theCameraMatrix.rotation * cameraToStart;
if(unscaledField.z >= 0.f)
{
//cout << "unscaledField.z: "<< unscaledField.z << endl;
return false; // above horizon
}
const float scaleFactor = (theCameraMatrix.translation.z - theFieldDimensions.ballRadius) / unscaledField.z;
/*
cout << "theFieldDimensions.ballRadius: " << theFieldDimensions.ballRadius << endl;
cout << "theCameraMatrix.translation.z: " << theCameraMatrix.translation.z << endl;
cout << "unscaledField.x: " << unscaledField.x << endl;
cout << "unscaledField.z: " << unscaledField.z << endl;
cout << "unscaledField.y: " << unscaledField.y << endl;
cout << "scaleFactor: " << scaleFactor << endl;
*/
cameraToStart *= scaleFactor;
unscaledField *= scaleFactor;
if(Vector2<>(unscaledField.x, unscaledField.y).squareAbs() > sqrMaxBallDistance)
{
/*
cout << "Vector2<>(unscaledField.x, unscaledField.y).squareAbs(): "<< Vector2<>(unscaledField.x, unscaledField.y).squareAbs() << endl;
cout << "sqrMaxBallDistance: " << sqrMaxBallDistance << endl;
cout << "too far away" << endl;
*/
return false; // too far away
}
cameraToStart.y += cameraToStart.y > 0 ? -theFieldDimensions.ballRadius : theFieldDimensions.ballRadius;
cameraToStart /= scaleFactor;
approxRadius1 = abs(theImageCoordinateSystem.fromCorrectedApprox(Vector2<int>(int(theCameraInfo.opticalCenter.x - cameraToStart.y), int(theCameraInfo.opticalCenter.y - cameraToStart.z))).x - spot.x);
return true;
}
