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