bool Transformation::robotToImage(const Vector3<>& point, const CameraMatrix& cameraMatrix,
const CameraInfo& cameraInfo, Vector2<float>& pointInImage)
{
Vector3<> pointInCam = cameraMatrix.invert() * point;
if(pointInCam.x < 0)
{
return false;
}
pointInCam *= cameraInfo.focalLength / pointInCam.x;
pointInImage = cameraInfo.opticalCenter - Vector2<>(pointInCam.y, pointInCam.z);
return pointInCam.x > 0;
}
bool Geometry::calculatePointInImage
(
const Vector3<>& point,
const CameraMatrix& cameraMatrix,
const CameraInfo& cameraInfo,
Vector2<>& pointInImage
)
{
Vector3<> pointInCam = cameraMatrix.invert() * point;
if(pointInCam.x == 0)
{
return false;
}
pointInCam *= cameraInfo.focalLength / pointInCam.x;
pointInImage = cameraInfo.opticalCenter - Vector2<>(pointInCam.y, pointInCam.z);
return pointInCam.x > 0;
}
Matrix2x2f UKFSample::getCovOfPointInWorld(const Vector2<>& pointInWorld2, float pointZInWorld, const MotionInfo& motionInfo,
const CameraMatrix& cameraMatrix, const SelfLocatorParameters& parameters) const
{
Vector3<> unscaledVectorToPoint = cameraMatrix.invert() * Vector3<>(pointInWorld2.x, pointInWorld2.y, pointZInWorld);
const Vector3<> unscaledWorld = cameraMatrix.rotation * unscaledVectorToPoint;
const float h = cameraMatrix.translation.z - pointZInWorld;
const float scale = h / -unscaledWorld.z;
Vector2f pointInWorld(unscaledWorld.x * scale, unscaledWorld.y * scale);
const float distance = pointInWorld.abs();
Vector2f cossin = distance == 0.f ? Vector2f(1.f, 0.f) : pointInWorld * (1.f / distance);
Matrix2x2f rot(cossin, Vector2f(-cossin.y, cossin.x));
const Vector2<>& robotRotationDeviation = motionInfo.motion == MotionRequest::stand
? parameters.robotRotationDeviationInStand : parameters.robotRotationDeviation;
Matrix2x2f cov(Vector2f(sqr(h / tan((distance == 0.f ? pi_2 : atan(h / distance)) - robotRotationDeviation.x) - distance), 0.f),
Vector2f(0.f, sqr(tan(robotRotationDeviation.y) * distance)));
return rot * cov * rot.transpose();
}
CameraMatrix CameraMatrix::inverse() const {
CameraMatrix cm = *this;
cm.invert();
return cm;
}
void CameraMatrixProvider::drawFieldLines(const CameraMatrix& cameraMatrix, const RobotPose& theRobotPose, const FieldDimensions& theFieldDimensions, const CameraInfo& theCameraInfo) const
{
Vector3<> start0C, start1C, end0C, end1C;
Vector2<> start0I, start1I, end0I, end1I;
const Pose2D& robotPoseInv(theRobotPose.invert());
const Pose3D& cameraMatrixInv(cameraMatrix.invert());
int halfFieldLinesWidth = theFieldDimensions.fieldLinesWidth / 2;
for(unsigned int i = 0; i < theFieldDimensions.fieldLines.lines.size(); ++i)
{
Pose2D relativeLine(robotPoseInv);
relativeLine.conc(theFieldDimensions.fieldLines.lines[i].corner);
const Vector2<> start0(Pose2D(relativeLine).translate(0, (float) - halfFieldLinesWidth).translation);
const Vector2<> end1(Pose2D(relativeLine).translate(theFieldDimensions.fieldLines.lines[i].length, (float) halfFieldLinesWidth).translation);
start0C = cameraMatrixInv * Vector3<>(start0.x, start0.y, 0.); // field2camera
end1C = cameraMatrixInv * Vector3<>(end1.x, end1.y, 0.); // field2camera
if(start0C.x <= 200 && end1C.x <= 200)
continue;
const Vector2<>& start1(Pose2D(relativeLine).translate(0, (float) halfFieldLinesWidth).translation);
const Vector2<>& end0(Pose2D(relativeLine).translate(theFieldDimensions.fieldLines.lines[i].length, (float) - halfFieldLinesWidth).translation);
start1C = cameraMatrixInv * Vector3<>(start1.x, start1.y, 0.); // field2camera
end0C = cameraMatrixInv * Vector3<>(end0.x, end0.y, 0.); // field2camera
if(start0C.x <= 200)
intersectLineWithCullPlane(start0C, end0C - start0C, start0C);
else if(end0C.x <= 200)
intersectLineWithCullPlane(start0C, end0C - start0C, end0C);
if(start1C.x <= 200)
intersectLineWithCullPlane(start1C, end1C - start1C, start1C);
else if(end1C.x <= 200)
intersectLineWithCullPlane(start1C, end1C - start1C, end1C);
camera2image(start0C, start0I, theCameraInfo);
camera2image(end0C, end0I, theCameraInfo);
camera2image(start1C, start1I, theCameraInfo);
camera2image(end1C, end1I, theCameraInfo);
//LINE("module:CameraMatrixProvider:calibrationHelper", start0I.x, start0I.y, end0I.x, end0I.y, 0, Drawings::ps_solid, ColorRGBA(0, 0, 0));
//LINE("module:CameraMatrixProvider:calibrationHelper", start1I.x, start1I.y, end1I.x, end1I.y, 0, Drawings::ps_solid, ColorRGBA(0, 0, 0));
}
start0C = cameraMatrixInv * Vector3<>(100, -11, 0.); // field2camera
end0C = cameraMatrixInv * Vector3<>(0, -11, 0.); // field2camera
camera2image(start0C, start0I, theCameraInfo);
camera2image(end0C, end0I, theCameraInfo);
//LINE("module:CameraMatrixProvider:calibrationHelper", start0I.x, start0I.y, end0I.x, end0I.y, 0, Drawings::ps_solid, ColorClasses::blue);
start0C = cameraMatrixInv * Vector3<>(100, 11, 0.); // field2camera
end0C = cameraMatrixInv * Vector3<>(0, 11, 0.); // field2camera
camera2image(start0C, start0I, theCameraInfo);
camera2image(end0C, end0I, theCameraInfo);
//LINE("module:CameraMatrixProvider:calibrationHelper", start0I.x, start0I.y, end0I.x, end0I.y, 0, Drawings::ps_solid, ColorClasses::blue);
start0C = cameraMatrixInv * Vector3<>(110, 1000, 0.); // field2camera
end0C = cameraMatrixInv * Vector3<>(110, -1000, 0.); // field2camera
camera2image(start0C, start0I, theCameraInfo);
camera2image(end0C, end0I, theCameraInfo);
//LINE("module:CameraMatrixProvider:calibrationHelper", start0I.x, start0I.y, end0I.x, end0I.y, 0, Drawings::ps_solid, ColorClasses::blue);
}