Pose estimation from 2 points and known z-axis.

182 Views Asked by At

I'm trying to solve this projection problem. Basically I have two points and I want to estimate the camera pose from it. As additional information, I know that R is a rotation around the z-axis.

$$ \lambda^1\cdot p^1 = R_z(\phi)P^1 + T \\ \lambda^2\cdot p^2 = R_z(\phi) P^2 + T $$

In homogeneous coordinates the problem looks like this:

$$ \begin{pmatrix} \lambda^1\cdot p_x^1 \\ \lambda^1\cdot p_y^1 \\ \lambda^1\cdot p_z^1\\1\end{pmatrix} = \begin{pmatrix} \cos(\phi) & -\sin(\phi) & 0 & T_x \\ \sin(\phi) & \cos(\phi) & 0 & T_y \\ 0 & 0 & 1 & T_z\\ 0 & 0 & 0 & 1\end{pmatrix}\cdot\begin{pmatrix} P_x^1 \\ P_y^1 \\ P_z^1 \\ 1 \end{pmatrix} $$ and $$ \begin{pmatrix} \lambda^2\cdot p_x^2 \\ \lambda^2\cdot p_y^2 \\ \lambda^2\cdot p_z^2\\1\end{pmatrix} = \begin{pmatrix} \cos(\phi) & -\sin(\phi) & 0 & T_x \\ \sin(\phi) & \cos(\phi) & 0 & T_y \\ 0 & 0 & 1 & T_z\\ 0 & 0 & 0 & 1\end{pmatrix}\cdot\begin{pmatrix} P_x^2 \\ P_y^2 \\ P_z^2 \\ 1 \end{pmatrix} $$ with $ \lambda^1 = \frac{P_z^1 + T_z}{p_z^1} $ and $ \lambda^2 = \frac{P_z^2 + T_z}{p_z^2} $, this results in the following equation system:

$$ P_x^1\cos(\phi) - P_y^1\sin(\phi) + T_x - (P_z^1 + T_z)\cdot \frac{p_x^1}{p_z^1} = 0 \\ P_x^1\sin(\phi) + P_y^1\cos(\phi) + T_y - (P_z^1 + T_z)\cdot \frac{p_y^1}{p_z^1} = 0 \\ P_x^2\cos(\phi) - P_y^2\sin(\phi) + T_x - (P_z^2 + T_z)\cdot \frac{p_x^2}{p_z^2} = 0 \\ P_x^2\sin(\phi) + P_y^2\cos(\phi) + T_y - (P_z^2 + T_z)\cdot \frac{p_y^2}{p_z^2} = 0$$ I want to compute $\phi$ and $T$. Any ideas how I could solve this?

Is it possible to solve this analytically?

If I solve this numerically, how can I be sure that the solution is the one I want?

I would appreciate any insight, thank's in advance!

1

There are 1 best solutions below

0
On BEST ANSWER

EDIT Just as I got ready to post this, I saw that you had edited your question to change the equations somewhat. As far as I can see, this doesn't change the thrust of my answer, though some of the formulas are now wrong (if they weren't before.) I don't feel up to going through and figuring out what needs to change. I think you'll be able to follow the approach I give yourself, if you decide to adopt it. In any event, you ought to do this, because as I say below, my typing is terrible.

Note that once we know $\phi,$ we have four linear equations in three unknowns. Presumably, these will be consistent, and we can just solve for $T_x, T_y, \text{ and } T_z.$

Subtracting the third equation from the first gives $$P_x \cos \phi -P_y \sin \phi - p_x T_z = K_1, \tag{1}$$ where I have written

$$\begin{align} P_x &= P_x^1- P_x^2,\\ P_y &= P_y^1- P_y^2,\\ p_x &= p_x^1- p_x^2,\\ K_1 &= p_x^1P_z^1- p_x^2P_z^2,\\ \end{align}$$

Similarly, subtracting the fourth equation from the second gives $$P_x \sin \phi +P_y \cos \phi - p_y T_z = K_2, \tag{2}$$ where $K_2$ has a definition similar to that of $K_1$.

Now obviously we can eliminate $T_z$ from (1) and (2), and get an equation of the form $$a \cos \phi + b \sin \phi + c = 0, \tag{3}$$ for constants $a,b,c$ which are easy to compute but which I am unlikely to type correctly. (BTW, please check all my formulas very closely. I am a terrible typist.)

Now (3) can be solved numerically. If you want an "analytic" solution you can rewrite it as $a \cos \phi = -b \sin \phi -c,$ square both sides and use $\sin^2 \phi + \cos^2\phi =1$ to get a quadratic in $\sin \phi$. After solving it, we can, of course compute $\phi.$ We will, in general, get two values for $\sin \phi$ and each of these will correspond to two possible values of $0 \le \phi < 2\pi.$

I don't know enough about the problem to know how you will be able to tell which angle is the one you want. You may have to have some kind of human intervention, before or after the computation.