# Image coordinate to robot coordinate

I am working with a Nao robot. One if the things I want to do is the following:

• Take an image of geometrical objects in front of the Nao (done)
• Extract features from the objects in the image, such as x, y, color, etc.. (done)
• Make the Nao point to one of the objects (TO DO)

So, what I need is to transform a 2D coordinate in the image plane to a 3D coordinate in the robot's coordinate system. I have some idea on how to do this, but I am not sure if this is correct.

So, I start by transforming the 2D image coordinate to the 3D coordinate system that originates in the camera of the Nao. For this, I use height and width (in pixels) of the image ($r_w$ and $r_h$) and the horizontal and vertical camera opening ($\theta_v$ and $\theta_h$). This transformation is: $$v_c = \left( \begin{matrix} 1 \\ -\frac{x_i}{r_h} \tan{\frac{\theta_h}{2}} \\ \frac{y_i}{r_w} \tan{\frac{\theta_v}{2}} \end{matrix} \right)$$

Than, I need to translate this vector using the orientation of the head of the Nao. The resulting coordinate system is parallel to the entire robot's coordinate system. The rotation of the camera is given by $R_c$. The translation is: $$v_t = R_c v_c$$

Finally, I project this coordinate system to the robot's coordinate system. The Nao can use 2 coordinate system, one that originates on the ground between its legs (FRAME_ROBOT) or one that originates in its chest (FRAME_TORSO). Which one is used has no real importance for me. Let's say the offset between the coordinate systems is given by $t_c$. The transformation is: $$v_r = v_t + t_c$$

So, given a position in the image $(x_i, y_i)$, I get this position in the robot's coordinate system. Is this a correct approach?