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;
}
示例#2
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;
}
示例#3
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();
}
示例#4
0
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);
}