# Converting Image Coordinates to (x, y) Position for Robotic Arm

I have a Robotic Arm with a camera mounted above it looking down at a slight angle.

Assuming I know the height of the camera, the angle of tilt and the small distance from the center of the robot which is considered (0, 0) what else would I need to convert the image coordinates to the distance from the center of the robotic arm?

I am also assuming all the objects will have a z=0 because they will be sitting on the same platform as the arm.

I have the inverse kinematics worked out to control the arm, I just need to give it coordinates to move to.

If it helps I am using Python and OpenCV.

Edit: Clarification

• Image coordinates will correspond to a vector in world space. The vector will go from the camera image origin, through the pixel coordinates in the image plane. How will you know where along the vector you want the arm to go?
– Ben
May 8 '17 at 1:28

A quite simple math is involved in this task, but you need to be aware of the pinhole camera model as well as of the homographic projection.

The pinhole camera model gives you the 3D position $(x,y,z)$ of a point in the space whose projection in the image plane corresponds to the $(u,v)$ pixel at a distance $\lambda$ from the image plane (considered infinite):

$$\left( \begin{array}{c} x \\ y \\ z \\ 1 \end{array} \right) = \Pi^\dagger \cdot \left( \begin{array}{c} \lambda \cdot u \\ \lambda \cdot v \\ \lambda \end{array} \right),$$

where $\Pi^\dagger$ is the pseudoinverse of the matrix $\Pi \in \mathbb{R}^{3 \times 4}$ that incorporates both the intrinsic parameters of your camera (i.e. the focal length, the pixel ratio and the position of the principal point) and the extrinsic parameters accounting for the position of the camera reference frame expressed in the world frame (i.e. a $\mathbb{R}^{4 \times 4}$ roto-translation matrix).

You have now to use the concept of homography, because you know that the object of interest lies on the plane $z=0$.

To do so, let's define the following quantities:

• $\mathbf{p}_0$, a point in the plane $z=0$; in particular, $\mathbf{p}_0=(0,0,0)$ is fine.
• $\mathbf{z}$, the normal to the plane $z=0$, that is $\mathbf{z}=(0,0,1)$.
• $\mathbf{c}$, the 3D position of the camera frame expressed in the world frame.
• $\mathbf{p}_1$, a 3D point that corresponds to the pixel $(u,v)$ at a distance $\lambda=1$.

Your final point $\mathbf{p}^*$ is therefore:

$$\mathbf{p}^*=\mathbf{c}+\frac{(\mathbf{p}_0-\mathbf{c}) \cdot \mathbf{z}}{(\mathbf{p}_1-\mathbf{c}) \cdot \mathbf{z}} (\mathbf{p}_1-\mathbf{c}).$$

That said, the strong assumption we're making here is that the object of interest is squashed on the plane $z=0$, that is it has a null height, which is not reasonable. To compensate for that, you could include such an information in the above process and replace the plane $z=0$ with $z=h$, where $h$ represents a suitable height.