void CameraControlEngine::calculateYImageCoordinate(const Vector3<>& hip2Target,
const Pose3D& transformMatrix, const Pose3D& transformMatrixOther,
float& yImage, float& yImageOther)
{
const Pose3D transformMatrixInv = transformMatrix.invert();
const Pose3D transformMatrixOtherInv = transformMatrixOther.invert();
// We don't want points to be behind the camera. Since we only need the y image coordinate,
// we pretend lies directly in front of the camera.
const float absInPlane = sqrt(sqr(hip2Target.x) + sqr(hip2Target.y));
const Vector3<> fakeTarget(absInPlane * cos(theFilteredJointData.angles[JointData::HeadYaw]),
absInPlane * sin(theFilteredJointData.angles[JointData::HeadYaw]),
hip2Target.z);
const Vector3<> fakeTargetCamera = transformMatrixInv * fakeTarget;
const Vector3<> fakeTargetCameraOther = transformMatrixOtherInv * fakeTarget;
yImage = -fakeTargetCamera.z * theCameraInfo.focalLength / fakeTargetCamera.x;
yImageOther = -fakeTargetCameraOther.z * theCameraInfo.focalLength / fakeTargetCameraOther.x;
}
bool Transformation::imageToRobotWithCameraRotation(const Vector2<int>& pointInImage,
const CameraMatrix& cameraMatrix, const CameraInfo& cameraInfo,
Vector2<>& relativePosition)
{
const bool ret = imageToRobot(pointInImage, cameraMatrix, cameraInfo, relativePosition);
if(ret)
{
Pose3D cameraRotatedMatrix;
cameraRotatedMatrix.rotateZ(cameraMatrix.rotation.getZAngle());
const Vector3<> point3D = cameraRotatedMatrix.invert() * Vector3<>(relativePosition.x, relativePosition.y, 0.f);
relativePosition.x = point3D.x;
relativePosition.y = point3D.y;
}
return ret;
}