3D几何将空间中的点投影到给定FOV和角度的图像

Question

我正在尝试将笛卡尔空间中的3D点投影到图像上，因此如果某个相机正在查看它，它将在图像上的位置显示为圆圈。

知道： 摄像机具有水平视野，垂直视场，方位角和仰角。 它位于（0,0,0），虽然能够改变这将是好的。 图像具有一定的大小。 空间中的点具有已知的X，Y，Z位置。

我正在使用非常好的C ++库MathGeoLib

在我的尝试中，更改方位角或高程不起作用，但如果忽略旋转，投影似乎正常。

如何将现实世界点转换为图像的像素位置，前提是您知道相机的位置，视图的方向以及视野？

Frustum MyClass::GetCameraFrustrum()
{
  Frustum frustrum;
  frustrum.SetPerspective(DegToRad(_horizontal_field_of_view), DegToRad(_vertical_filed_of_view));
  // Looking straight down the z axis
  frustrum.SetFront(vec(0, 0, 1));
  frustrum.SetUp(vec(0, 1, 0));
  frustrum.SetPos(vec(0,0,0));
  frustrum.SetKind(FrustumSpaceGL, FrustumRightHanded);

  frustrum.SetViewPlaneDistances(1, 2000);
  float3x3 rotate_around_x = float3x3::RotateX(DegToRad(_angle_azimuth));
  frustrum.Transform(rotate_around_x);

  float3x3 rotate_around_y = float3x3::RotateY(DegToRad(_angle_elevation));
  frustrum.Transform(rotate_around_y);
  return frustrum;
}

void MyClass::Test()
{
  cv::Mat image = cv::Mat::zeros(2000, 2000, CV_8UC1);
  Frustum camera_frustrum = GetCameraFrustrum();
  vec projection = camera_frustrum.Project(_position_of_object);

  cv::Point2d location_on_image(projection.x * image.cols, projection.y * image.rows);

  LOG_DEBUG << location_on_image;
  cv::circle(image, location_on_image, 5, cv::Scalar(255, 0, 0), 3, cv::LINE_AA);
}