Here is the statement of the problem
Given three points $P_1, P_2, P_3$ in space with known coordinates, a camera (pinhole camera) with known focal length is used to take an image of these three points. In the image their coordinates are $p_1 , p_2, p_3$ respectively, which are also known. I'd like to determine the position of the camera in space and its orientation with respect to the world coordinate reference frame.
My approach:
I related the world frame coordinates and the image coordinates by
$ P_i = r_0 + t_i R p_i \hspace{10pt}(1)$
where $r_0 $ is the postion of the pinhole of the camera, and $R$ is its orientation with respect to the world Cartesian frame.
Taking pairs of points, we can write
$P_2 - P_1 = R (t_2 p_2 - t_1 p_1) \hspace{10pt}(2)$
$P_3 - P_1 = R (t_3 p_3 - t_1 p_1) \hspace{10pt}(3)$
$P_3 - P_2 = R (t_3 p_3 - t_2 p_2) \hspace{10pt}(4)$
The left hand sides are known vectors, and the right hand contains the unknowns $t_1, t_2, t_3$ and the rotation matrix $R$. Since $R$ is a rotation matrix, it does not affect the length; therefore, we can write
$\| P_2 - P_1 \|^2 = d_{12}^2 = (t_2 p_2 - t_1 p_1)^T (t_2 p_2 - t_1 p_1)\hspace{10pt}(5) $
$\| P_3 - P_1 \|^2 = d_{13}^2 = (t_3 p_3 - t_1 p_1)^T (t_3 p_3 - t_1 p_1) \hspace{10pt}(6)$
$\| P_3 - P_2 \|^2 = d_{23}^2 = (t_3 p_3 - t_2 p_2)^T (t_3 p_3 - t_2 p_2) \hspace{10pt}(7)$
And these are three quadratic equations in $t_1, t_2, t_3$, which can be solved numerically for $t_1, t_2, t_3$.
Plugging these values of $t_1, t_2, t_3$ in equations $(2),(3)$, they become
$ V_1 = P_2 - P_1 = R W_1 \hspace{10pt}(8) \hspace{15pt} \text{where} \hspace{5pt} W_1 = t_2 p_2 - t_1 p_1 $
And similarly
$V_2 = P_3 - P_1 = R W_2 \hspace{10pt}(9) \hspace{15pt} \text{where} \hspace{5pt} W_2 = t_3 p_3 - t_1 p_1 $
Since $R$ is a rotation matrix, then it follows that
$ V_1 \times V_2 = R ( W_1 \times W_2 ) \hspace{10pt}(10)$
Equations $(8), (9), (10) $ can be solved for $R$.
Finally, from equation $(1)$,
$P_1 = r_0 + t_1 R p_1 $
Substituting $t_1$ and $R$ , we can directly solve for $r_0$.
And this is how I found the position $r_0$ and the orientation $R$ of the camera.