void CoordinateSystemProvider::update(ImageCoordinateSystem& imageCoordinateSystem)
{
Geometry::Line horizon = Geometry::calculateHorizon(theCameraMatrix, theCameraInfo);
imageCoordinateSystem.origin = horizon.base;
imageCoordinateSystem.rotation.c[0] = horizon.direction;
imageCoordinateSystem.rotation.c[1] = Vector2<double>(-horizon.direction.y, horizon.direction.x);
imageCoordinateSystem.invRotation = imageCoordinateSystem.rotation.transpose();
RotationMatrix r(theCameraMatrix.rotation.transpose() * prevCameraMatrix.rotation);
imageCoordinateSystem.offset = Vector2<double>(r.getZAngle(), r.getYAngle());
calcScaleFactors(imageCoordinateSystem.a, imageCoordinateSystem.b);
imageCoordinateSystem.offsetInt = Vector2<int>(int(imageCoordinateSystem.offset.x * 1024 + 0.5),
int(imageCoordinateSystem.offset.y * 1024 + 0.5));
imageCoordinateSystem.aInt = int(imageCoordinateSystem.a * 1024 + 0.5);
imageCoordinateSystem.bInt = int(imageCoordinateSystem.b * 1024 + 0.5);
imageCoordinateSystem.cameraInfo = theCameraInfo;
prevCameraMatrix = theCameraMatrix;
prevTime = theFilteredJointData.timeStamp;
DECLARE_DEBUG_DRAWING("horizon", "drawingOnImage"); // displays the horizon
ARROW("horizon",
imageCoordinateSystem.origin.x,
imageCoordinateSystem.origin.y,
imageCoordinateSystem.origin.x + imageCoordinateSystem.rotation.c[0].x * 50,
imageCoordinateSystem.origin.y + imageCoordinateSystem.rotation.c[0].y * 50,
1, Drawings::ps_solid, ColorRGBA(255,0,0));
ARROW("horizon",
imageCoordinateSystem.origin.x,
imageCoordinateSystem.origin.y,
imageCoordinateSystem.origin.x + imageCoordinateSystem.rotation.c[1].x * 50,
imageCoordinateSystem.origin.y + imageCoordinateSystem.rotation.c[1].y * 50,
1, Drawings::ps_solid, ColorRGBA(255,0,0));
GENERATE_DEBUG_IMAGE(corrected,
INIT_DEBUG_IMAGE_BLACK(corrected);
int yDest = -imageCoordinateSystem.toCorrectedCenteredNeg(0, 0).y;
for(int ySrc = 0; ySrc < theImage.cameraInfo.resolutionHeight; ++ySrc)
for(int yDest2 = -imageCoordinateSystem.toCorrectedCenteredNeg(0, ySrc).y; yDest <= yDest2; ++yDest)
{
int xDest = -imageCoordinateSystem.toCorrectedCenteredNeg(0, ySrc).x;
for(int xSrc = 0; xSrc < theImage.cameraInfo.resolutionWidth; ++xSrc)
{
for(int xDest2 = -imageCoordinateSystem.toCorrectedCenteredNeg(xSrc, ySrc).x; xDest <= xDest2; ++xDest)
{
DEBUG_IMAGE_SET_PIXEL_YUV(corrected, xDest + int(theCameraInfo.opticalCenter.x + 0.5),
yDest + int(theCameraInfo.opticalCenter.y + 0.5),
theImage.image[ySrc][xSrc].y,
theImage.image[ySrc][xSrc].cb,
theImage.image[ySrc][xSrc].cr);
}
}
}
SEND_DEBUG_IMAGE(corrected);
);
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;
}
bool BallPerceptor::calculateBallOnField(const ImageCoordinateSystem& theImageCoordinateSystem,
const CameraInfo& theCameraInfo,
const FieldDimensions& theFieldDimensions,
const CameraMatrix& theCameraMatrix)
{
const Vector2<> correctedCenter = theImageCoordinateSystem.toCorrected(center);
Vector3<> cameraToBall(theCameraInfo.focalLength, theCameraInfo.opticalCenter.x - correctedCenter.x, theCameraInfo.opticalCenter.y - correctedCenter.y);
cameraToBall.normalize(theFieldDimensions.ballRadius * theCameraInfo.focalLength / radius);
Vector3<> rotatedCameraToBall = theCameraMatrix.rotation * cameraToBall;
sizeBasedCenterOnField = theCameraMatrix.translation + rotatedCameraToBall;
bearingBasedCenterOnField = theCameraMatrix.translation - rotatedCameraToBall * ((theCameraMatrix.translation.z - theFieldDimensions.ballRadius) / rotatedCameraToBall.z);
//CIRCLE("module:BallPerceptor:field", sizeBasedCenterOnField.x, sizeBasedCenterOnField.y, theFieldDimensions.ballRadius, 1, Drawings::ps_solid, ColorRGBA(0, 0, 0xff), Drawings::bs_null, ColorRGBA());
//CIRCLE("module:BallPerceptor:field", bearingBasedCenterOnField.x, bearingBasedCenterOnField.y, theFieldDimensions.ballRadius, 1, Drawings::ps_solid, ColorRGBA(0xff, 0, 0), Drawings::bs_null, ColorRGBA());
usedCenterOnField = rotatedCameraToBall.z < 0 ? bearingBasedCenterOnField : sizeBasedCenterOnField;
return true;
}
void CognitionLogDataProvider::update(ImageCoordinateSystem& imageCoordinateSystem)
{
imageCoordinateSystem.setCameraInfo(theCameraInfo);
DECLARE_DEBUG_DRAWING("loggedHorizon", "drawingOnImage"); // displays the horizon
ARROW("loggedHorizon",
imageCoordinateSystem.origin.x(),
imageCoordinateSystem.origin.y(),
imageCoordinateSystem.origin.x() + imageCoordinateSystem.rotation(0, 0) * 50,
imageCoordinateSystem.origin.y() + imageCoordinateSystem.rotation(1, 0) * 50,
0, Drawings::solidPen, ColorRGBA(255, 0, 0));
ARROW("loggedHorizon",
imageCoordinateSystem.origin.x(),
imageCoordinateSystem.origin.y(),
imageCoordinateSystem.origin.x() + imageCoordinateSystem.rotation(0, 1) * 50,
imageCoordinateSystem.origin.y() + imageCoordinateSystem.rotation(1, 1) * 50,
0, Drawings::solidPen, ColorRGBA(255, 0, 0));
COMPLEX_IMAGE(corrected)
{
if(Blackboard::getInstance().exists("Image"))
{
const Image& image = (const Image&) Blackboard::getInstance()["Image"];
INIT_DEBUG_IMAGE_BLACK(corrected, theCameraInfo.width, theCameraInfo.height);
int yDest = -imageCoordinateSystem.toCorrectedCenteredNeg(0, 0).y();
for(int ySrc = 0; ySrc < theCameraInfo.height; ++ySrc)
for(int yDest2 = -imageCoordinateSystem.toCorrectedCenteredNeg(0, ySrc).y(); yDest <= yDest2; ++yDest)
{
int xDest = -imageCoordinateSystem.toCorrectedCenteredNeg(0, ySrc).x();
for(int xSrc = 0; xSrc < theCameraInfo.width; ++xSrc)
{
for(int xDest2 = -imageCoordinateSystem.toCorrectedCenteredNeg(xSrc, ySrc).x(); xDest <= xDest2; ++xDest)
{
DEBUG_IMAGE_SET_PIXEL_YUV(corrected, xDest + int(theCameraInfo.opticalCenter.x() + 0.5f),
yDest + int(theCameraInfo.opticalCenter.y() + 0.5f),
image[ySrc][xSrc].y,
image[ySrc][xSrc].cb,
image[ySrc][xSrc].cr);
}
}
}
SEND_DEBUG_IMAGE(corrected);
}
}
}
void CoordinateSystemProvider::update(ImageCoordinateSystem& imageCoordinateSystem)
{
imageCoordinateSystem.setCameraInfo(theCameraInfo);
Geometry::Line horizon = Geometry::calculateHorizon(theCameraMatrix, theCameraInfo);
imageCoordinateSystem.origin = horizon.base;
imageCoordinateSystem.rotation.col(0) = horizon.direction;
imageCoordinateSystem.rotation.col(1) = Vector2f(-horizon.direction.y(), horizon.direction.x());
imageCoordinateSystem.invRotation = imageCoordinateSystem.rotation.transpose();
const CameraMatrix& cmPrev = cameraMatrixPrev[theCameraInfo.camera];
RotationMatrix r(theCameraMatrix.rotation.inverse() * cmPrev.rotation);
imageCoordinateSystem.offset = Vector2f(r.getZAngle(), r.getYAngle());
calcScaleFactors(imageCoordinateSystem.a, imageCoordinateSystem.b, theJointSensorData.timestamp - cameraMatrixPrevTimeStamp[theCameraInfo.camera]);
cameraMatrixPrev[theCameraInfo.camera] = theCameraMatrix;
cameraMatrixPrevTimeStamp[theCameraInfo.camera] = theJointSensorData.timestamp;
COMPLEX_IMAGE(corrected)
{
INIT_DEBUG_IMAGE_BLACK(corrected, theCameraInfo.width, theCameraInfo.height);
int yDest = -imageCoordinateSystem.toCorrectedCenteredNeg(0, 0).y();
for(int ySrc = 0; ySrc < theImage.height; ++ySrc)
for(int yDest2 = -imageCoordinateSystem.toCorrectedCenteredNeg(0, ySrc).y(); yDest <= yDest2; ++yDest)
{
int xDest = -imageCoordinateSystem.toCorrectedCenteredNeg(0, ySrc).x();
for(int xSrc = 0; xSrc < theImage.width; ++xSrc)
{
for(int xDest2 = -imageCoordinateSystem.toCorrectedCenteredNeg(xSrc, ySrc).x(); xDest <= xDest2; ++xDest)
{
DEBUG_IMAGE_SET_PIXEL_YUV(corrected, xDest + int(theCameraInfo.opticalCenter.x() + 0.5f),
yDest + int(theCameraInfo.opticalCenter.y() + 0.5f),
theImage[ySrc][xSrc].y,
theImage[ySrc][xSrc].cb,
theImage[ySrc][xSrc].cr);
}
}
}
SEND_DEBUG_IMAGE(corrected);
}
COMPLEX_IMAGE(horizonAligned)
{
INIT_DEBUG_IMAGE_BLACK(horizonAligned, theCameraInfo.width, theCameraInfo.height);
for(int ySrc = 0; ySrc < theCameraInfo.height; ++ySrc)
for(int xSrc = 0; xSrc < theCameraInfo.width; ++xSrc)
{
Vector2f corrected(imageCoordinateSystem.toCorrected(Vector2i(xSrc, ySrc)));
corrected.x() -= theCameraInfo.opticalCenter.x();
corrected.y() -= theCameraInfo.opticalCenter.y();
const Vector2f& horizonAligned(imageCoordinateSystem.toHorizonAligned(corrected));
DEBUG_IMAGE_SET_PIXEL_YUV(horizonAligned, int(horizonAligned.x() + theCameraInfo.opticalCenter.x() + 0.5f),
int(horizonAligned.y() + theCameraInfo.opticalCenter.y() + 0.5f),
theImage[ySrc][xSrc].y,
theImage[ySrc][xSrc].cb,
theImage[ySrc][xSrc].cr);
}
SEND_DEBUG_IMAGE(horizonAligned);
}
}