# Mapping between camera pose and image features in visual servoing

I have a robotic arm and a camera in eye-in-hand configuration. I know that there is a relationship between the body velocity $V$ of the camera and the velocities $\dot s$ in the image feature space that is $\dot s=L(z,s) V$ where $L$ is the interaction matrix. I was wondering if one can find a mapping (a so called diffeomorphism) that connects the image features' vector $s$ with the camera pose $X$. All I was able to find is that it is possible to do that in a structured environment which I don't fully understand what it is.

Image features are connected to the camera pose through two steps: (1) the relationship between the feature pixel coordinates and its homogeneous coordinates in the camera reference frame, and (2) the relationship between the camera reference frame and the world frame. Take a look at this figure, where the world frame is denoted with subscript 0 and the camera frame is denoted with subscript C. A feature is shown as a blue dot, with position $p$ in the world frame. The camera has a particular field of view (shown with a pyramid in the image), which relates the pixels coordinates to the relative position of the feature in the camera reference frame, $\tilde{p}$, through the camera projection matrix:

$\begin{bmatrix} I \\ J \\ 1 \end{bmatrix} = \begin{bmatrix} k_x & 0 & C_x \\ 0 & k_y & C_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X \\ Y \\ 1 \end{bmatrix}$

Where $I$ and $J$ are the pixel coordinates of the feature in the image, and the camera is defined with parameters $k_x$, $k_y$, $C_x$ and $C_y$ (based on the field of view and output image size). The homogeneous coordinates, $X$ and $Y$ are defined based on the relative position of the feature in the camera frame:

$\tilde{p} = \begin{bmatrix} \tilde{x} \\ \tilde{y} \\ \tilde{z} \end{bmatrix}$

$X = \frac{\tilde{x}}{\tilde{z}}$

$Y = \frac{\tilde{y}}{\tilde{z}}$

That relative position of the feature is then related to the actual position of the feature (in the world frame) as well as the camera pose, according to:

$p = R_C \tilde{p} + p_C$

Where $R_C$ is the rotation matrix defining the camera orientation and $p_C$ is the position of the camera.

One thing I have not discussed is lens distortion, which must be accounted for before using this approach but can be treated separately. Note that the OpenCV Camera Calibration and 3D Reconstruction page provides a more detailed explanation.

• Nice introduction into pinhole camera model but I don't think this fully answers the question. It is impossible to estimate the velocity of features observed by a monocular camera without knowing their depth. It would be possible to do that by using structure from motion algorithms but it radically increases the complexity of the task. Using a kinect would be preferred. – Mehdi Nov 23 '15 at 9:41
• The question asks "if one can find a mapping... that connects the image features' vector s with the camera pose X". There is mention of velocities $\dot{s}$ but that is not the question. Also, this model necessarily requires 3D coordinates of features to begin with ($p$ is assumed to be known) -- I'm not saying you get depth from a monocular camera, these features are assumed to be known because it is a structured environment! Sorry if my answer is confusing, I will update it later. – Brian Lynch Nov 23 '15 at 9:54
• Structured environment means an environment with observable edges and corners as far as I know. Doesn't need to be an environment where the structure is known. I see that the question was only about the mapping from pixels to camera frame but I don't think that it would be enough to the OP considering his final goal. – Mehdi Nov 23 '15 at 9:58
• Not quite, a structured environment is one where features are recognizable based on some a priori knowledge. Yes, you can treat edges/corners as known structures in the environment, but in general it refers to the fact that these structures are filling a necessary gap. In the case of a monocular camera you would be depending on fiduciary markers or known objects with scale to provide that structure. Also, you often want to predict the pixel coordinates for a measurement model so this is the relevant component of that larger question -- what comes next may be already answered elsewhere. – Brian Lynch Nov 23 '15 at 10:26

Look up Camera Space Manipulation, which uses the pinhole camera model to map image space and physical space coordinates. It is not too difficult to do.

A structured environment is one in which objects which occupy the environment are known, and can be modelled. It is much easier to program mobility, and to identify objects, in a structured environment. For example, if you know that a particular circular object has a given diameter, you can use that object's elliptical shape in camera space to determine viewing angle, and dimensions to determine distance between the circle and the camera. But if you don't know the model of that object, you can't be sure if it is a circle viewed from an angle, or an ellipse viewed head-on. Therefore unstructured environments are much more difficult for relating camera space coordinates and physical coordinates.