我基本上想要的是一个物体的相对位置让我们说一个相对于相机镜头的立方体 . 因此，假设我将立方体放置在地面前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矩阵来提取cx，cy，fx，fy值，如here所述 . 投影/相机矩阵

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

按照惯例，该矩阵指定处理（整流）图像的固有（相机）矩阵 . 也就是说，左3x3部分是用于校正图像的普通相机固有矩阵 .

opencv 3d camera projection