How to implement object tracking in 3D using vectors?

Question

I am currently working on a project which tracks an object in 3D using two cameras. I am following a similar structure to this paper: Practical 3D Tracking Using Low-Cost Cameras. I have two cameras and have completed the calibration. I currently have an image like shown in my diagram:

From which I have the centre of the object. In the article they state that "The lines pointing towards the object from each camera give the location of the selected object in 3D". I am wondering what these vectors are and how to implement them. I am really stumped at the moment and any help would be much appreciated.

P.S

To be clear this question is specifically relating to what these vectors and and how I find them and use them.

Answer 1

Here is what the math looks like in 2D. 3D is similar, you just have some extra angles. First, you need a reference frame. Assume camera positions are $(C_{1x}, C_{1y})$ and $(C_{2x}, C_{2y})$, and rotated in such a way that the centers of the image are looking in directions that make angles $\alpha_1$ and $\alpha_2$ with respect to the $x$ axis. When you process the image, the object is found at angles $\beta_1$ and $\beta_2$ from the axis in the center of the image. Then we just write the equation of the line containing the object and a camera: $$\tan(\alpha_i+\beta_i)=\frac{y-C_{iy}}{x-C_{ix}}$$ You have two equations (one for each camera) with the object at the intersection of the two lines, so you need to calculate the two unknowns $x$ and $y$.

Answer 2

Attach a reference coordinate frame to each camera

Then for the first camera, if the position of the object in the world coordinate system is $P$, and the position in the first camera coordinate system is $P'$ then

$P = d_1 + R_1 P' $

where $d_1$ is the position vector of the origin of the first camera coordinate system, and $R_1$ is a rotation matrix whose columns are the direction vectors of the three axes of the camera coordinate system with respect to the world coordinate system.

Now we can assume that the $z'$ axis of the first camera coordinate system is pointing towards the view field, and that the projection plane of the view is at $z' = z_{10} $, the image of point $P'$ in the projection plane is the intersection of the line $O'P'$ with the plane $z = z_{10}$. The equation of this line is

$Q(t) = t P' $

At $z' = z_{10} $, we have $t = \dfrac{z_{10}}{ P'_z} $

Therefore, the image of $P'$ is

$Q' = \begin{bmatrix} z_{10} \dfrac{P'_x} {P'_z} \\ z_{10} \dfrac{P'_y}{P'_z} \\ z_{10} \end{bmatrix}$

Now given $Q'_1 = (Q'_{1x}, Q'_{1y}, z_{10} ) $ possible positions $P'$ form a line whose parametric equation is

$P' = t ( Q'_{1x}, Q'_{1y}, z_{10} ) $

Thus point $P$ lies on the line (in the world coordinates)

$P(t_1) = d_1 + R_1 (t_1 [Q'_{1x}, Q'_{1y}, z_{10} ]^T ) $

Similarly for the second camera, we have

$P(t_2) = d_2 + R_2 ( t_2 [Q'_{2x} , Q'_{2y}, z_{20} ]^T )$

Now we have two lines expressed in the world coordinate system, which can intersect to find the values of the the parameters $t_1$ and $t_2$ by setting up a linear system of the three coordinates in terms of the unknowns $t_1$ and $t_2$.

The above procedure describes how to obtain the position of a point object using two cameras.

Tracking a moving point object is another problem, that is discussed below.

Suppose the point object has an image $Q' = (Q'_x, Q'_y, z_0)$

These coordinates define a vector in the world coordinates as follows

$P = d_0 + R Q' $

Assuming the camera is mounted on a $2$-DOF stand changing its horizontal orientation ($\phi$) and angle of elevation above the horizontal plane $\theta$, such that when $\phi = \theta = 0$ the camera is pointing in the positive $x$ direction.

Now the $z'$ axis of the camera is pointing forward, while the $x'$ axis is pointing to the left of the image plane, and the $y'$ axis is pointing in the upward direction.

With this convention, the rotation matrix $R$ is given by

$R = R_0 R_{y'} (\phi) R_{x'} (-\theta) $

where

$R_0 = \begin{bmatrix} 0 && 0 && 1 \\ 1&& 0 && 0 \\ 0 && 1 && 0 \end{bmatrix} $

and

$ R_{y'}(\phi) = \begin{bmatrix} c_1 && 0 && s_1 \\ 0 && 1 && 0 \\ -s_1 &&0 && c_1 \end{bmatrix}$

where $c_1 = \cos \phi$ and $s_1 = \sin \phi$

and

$R_{x'}(-\theta) =\begin{bmatrix} 1&& 0 && 0 \\ 0 && c_2 && s_2 \\ 0&& -s_2 && c_2 \end{bmatrix} $

where $c_2 = \cos \theta$ and $s_2 =\sin \theta$

Multiplying the three matrices together results in

$R =\begin{bmatrix} -s_1 && -c_1 s_2 && c_1 c_2 \\ c_2 && -s_1 s_2&& s_1 c_2 \\ 0 && c_2 && s_2 \end{bmatrix} $

Now we want to find $R_\text{Target}$ that will result in the image being $(0, 0, z_0)$ so we set $ R v = R_\text{Target} (0, 0, 1) \hspace{48pt} (*) $

where, $v$ is the unit vector in the direction $(Q'_x, Q'_y, z_0 )$

That is,

$v = \dfrac{1}{\sqrt{ {Q'_x}^2 + {Q'_y}^2 + {z_0}^2} } (Q'_x, Q'_y, z_0 ) = (v_x, v_y, v_z) $

With the known values of the current camera orientation, we can compute its $R$ and hence we can compute the unit vector $u = R v= (u_x, u_y, u_z)$

The right hand side of the above equation (equation $(*)$ ) is just $\begin{bmatrix} c_1 c_2 \\ s_1 c_2 \\ s_2 \end{bmatrix} $

Direct comparison results in the values of target $\phi$ and $\theta$

$\theta = \sin^{-1} u_z $

$\phi = \text{ATAN2}( u_x / \cos(\theta) , u_y / \cos(\theta) )$

Having found the exact values of $\phi$ and $\theta$ these values can be passed to the servo motors that control the $2$-DOF mount of each of the two cameras.