使用2d输入图像确定对象的3d位置

Question

我基本上想要的是一个对象的相对位置让我们说一个相对于相机镜头的立方体。因此，假设我将立方体放置在地面前20厘米（x3方向）在地面上（这意味着在x2方向向下10厘米）和在x1方向向下3厘米。

我们还假设一个简单的针孔相机模型。接下来，我使用了here所描述的公式。此外，我已经拥有了我的相机的以下数据： fx，fy（焦距）和cx，cy主要点。器（Tx =泰 - = 0）。

首先，我计算了立方体与相机的距离： 我只是假设我已经有焦距：F =（fx + fy）/ 2。但我不确定我是否可以轻松应对。然后我可以用以下方法计算距离：

D =（W * F）/ P（截距定理：P / F = W / D）

F：焦距

P：在一定距离D处测量的像素的立方体直径

W：以米为单位的实际立方体高度

我使用的第二步是pinhole wikipage:：

中描述的公式

  x3 = D; // as calculated above
  x1 = ((cube_middle_point_x_in_px - cx - Tx)*x3) / fx;
  x2 = ((cube_middle_point_y_in_px - cy - Ty)*x3) / fy;

似乎计算距离非常正常。在这部分我只是不明白如何使用相机参数fx，fy导致在简单拦截定理中我只有f而不是fx或fy。

在第二部分中，我得到了奇怪的结果，因为我得到了x2的正值。这意味着我的立方体有点飞行或？这根本不符合逻辑。然而，x3和x1值似乎没问题。

我使用的是一个真实的R200相机，其中包含以下ros信息：

header: 
  seq: 9349
  stamp: 
    secs: 1513684762
    nsecs:  32878312
  frame_id: "camera_rgb_optical_frame"
height: 480
width: 640
distortion_model: "plumb_bob"
D: [-0.07397269457578659, 0.07420530915260315, 0.0004570182354655117, 0.002167842583730817, 0.0]
K: [614.6105346679688, 0.0, 308.61798095703125, 0.0, 620.16015625, 248.9337921142578, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [614.6105346679688, 0.0, 308.61798095703125, 0.0, 0.0, 620.16015625, 248.9337921142578, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False

我使用P矩阵来提取here所描述的cx，cy，fx，fy值。 投影/相机矩阵

     [fx'  0  cx' Tx]
 P = [ 0  fy' cy' Ty]
     [ 0   0   1   0]

按照惯例，此矩阵指定内在（相机）矩阵   经处理（整流）的图像。也就是说，左边3x3部分   是经过校正的图像的正常相机固有矩阵。