title: Determining the 3d position of an object with a 2d input image
body: <p>what I basically want is the relative position of an object lets say a cube relatively to the camera lense. So assume I place the cube 20cm before my camera(x3 direction) on the ground (this means 10cm down in x2 direction) and 3cm in x1 direction. </p>
  
  <p><a href="https://i.stack.imgur.com/NLdrX.png" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/NLdrX.png" alt="Here is a picture (the camera is placed on a robot)"></a></p>
  
  <p>We also assume a simple pinhole camera model. Next I used the formulas as described <a href="https://en.wikipedia.org/wiki/Pinhole_camera_model" rel="nofollow noreferrer">here</a>. Moreover I already have the following data of my camera:
  fx,fy (focal lenghts) and the cx,cy principal point. (Tx=Ty=0).</p>
  
  <p>First of all I calculated the distance of the cube to the camera:
  I just assumed that I already have the focal length: F=(fx+fy)/2. But I am not sure If I can handle that so easy. Then I can just calculate the distance with:</p>
  
  <p>D = (W*F)/P  (intercept theorem: P/F=W/D)</p>
  
  <p>F: focus length</p>
  
  <p>P: cube diameter in pixel measured at a certain distance D</p>
  
  <p>W: real cube height in meters</p>
  
  <p>Second step I use is the formulas described on the <a href="https://en.wikipedia.org/wiki/Pinhole_camera_model" rel="nofollow noreferrer">pinhole wikipage:</a>:</p>
  
  <pre><code>  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;
  </code></pre>
  
  <p>It seems that calculating the Distance works quite ok. In this part I just do not understand how to use the camera parameters fx,fy cause in the simple intercept theorem I just have a f and not a fx or fy. </p>
  
  <p>In the second part I have strange results cause I get positive values for the x2. This means my cube is kind of flying or? This does not make sense. The x3 and x1 value however seem to be ok.</p>
  
  <p>I use a realsense R200 Camera, which has the following ros information message:</p>
  
  <pre><code>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
  </code></pre>
  
  <p>I used the P matrix to extract the cx,cy, fx,fy values as described <a href="http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html" rel="nofollow noreferrer">here</a>.
  Projection/camera matrix</p>
  
  <pre><code>     [fx'  0  cx' Tx]
   P = [ 0  fy' cy' Ty]
       [ 0   0   1   0]
  </code></pre>
  
  <p>By convention, this matrix specifies the intrinsic (camera) matrix
    of the processed (rectified) image. That is, the left 3x3 portion
    is the normal camera intrinsic matrix for the rectified image.</p>