Closed-form solution for 3D rotation angles given pre- and post-image

Question

I'm working on some math involving a pinhole camera model, and I've run in to the following problem: given only $x$, $y$, $z$, $A$, $B$, and $C$, I need to solve for the angles $\theta$ and $\phi$ in the following system of equations:

$$ \begin{bmatrix} A \\ B \\ C \end{bmatrix} = \begin{bmatrix} \cos\phi & 0 & -\sin\phi \\ 0 & 1 & 0 \\ \sin\phi & 0 & \cos\phi \end{bmatrix} \begin{bmatrix} \cos\theta & \sin\theta & 0 \\ -\sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\ z \end{bmatrix} $$

under the assumption that $[x\ y\ z]^T$ and $[A\ B\ C]^T$ are both unit vectors. I know that $\phi$ must be in the interval $[-\frac{\pi}{2}, \frac{\pi}{2}]$; the value of $\theta$ is unconstrained.

I am able to solve for $\theta$ and $\phi$ using Newton's method. Is there a closed-form solution for $\theta$ and $\phi$ in this case, and if so, what is it?

Answer 1

There are two possible solutions for each angle, only one of which will work best

$$\begin{aligned} \theta & = 2 \arctan\left( -\frac{\sqrt{x^2+y^2-B^2}+x}{y+B}\right) \\ \theta & = 2 \arctan\left( \frac{\sqrt{x^2+y^2-B^2}-x}{y+B}\right) \\ \end{aligned}$$

$$\begin{aligned} \varphi & = 2 \arctan\left( -\frac{\sqrt{A^2+C^2-z^2}+A}{z+C}\right) \\ \varphi & = 2 \arctan\left( \frac{\sqrt{A^2+C^2-z^2}-A}{z+C}\right) \\ \end{aligned}$$

Why?

I used the Tan Half Angle substitution for $\theta = 2 \arctan(t)$ and $\varphi = 2 \arctan(s)$, $$\cos(\theta) = \frac{1-t^2}{t^2+1}$$ and $$\sin(\theta) = \frac{2 t}{t^2+1}$$ and similarly for $\cos(\varphi)$ and $\sin(\varphi)$.

Then I solved the two quadratic equations from the y and z components of the equation

$${\rm Rot}_Y(\varphi)[A,B,C] = {\rm Rot}_Z(-\theta) [x,y,z]$$

I used the standard convention for the 3×3 rotation matrices ${\rm Rot}_Y()$ and ${\rm Rot}_Z()$

The two equations I solved are

$$B = y \cos \theta-x \sin \theta \\ C \cos\varphi-A \sin\varphi=z$$

which get transformed to

$$ B = y \frac{1-t^2}{1+t^2}-x \frac{2 t}{1+t^2} \\ C \frac{1-s^2}{1+s^2} - A \frac{2 s}{1+s^2} = z $$

or

$$ B (1+t^2)=y (1-t^2)-2 t x \\ C (1-s^2) - 2 A s = z (1+s^2) $$

with solution $$ t = \frac{\sqrt{x^2+y^2-B^2}-x}{y+B} \\ s = \frac{\sqrt{C^2+A^2-z^2}-A}{z+C} $$

Answer 2

Please check this with what you get by direct computation.

Step 1: Convert the vector $[x, y, z]$ into spherical coordinates as
$x=r\sin(\alpha)\cos(\beta)$
$y=r\sin(\alpha)\sin(\beta)$
$z=r\cos(\alpha)$

Step 2: After applying the matrices we get the following vector which is equated to the given output
$A=r\sin(\alpha+\theta)\cos(\beta-\phi)$
$B=r\sin(\alpha+\theta)\sin(\beta-\phi)$
$C=r\cos(\alpha+\theta)$

Now, convert the vector $[A,B,C]$ to the spherical coordinates and then the answer would hopefully become visible.