4 points in 3-d space (one known and three unknown)

Question

Problem in 3-d space important for computer vision.
We have four points: $P_0$ where we know coordinates $(0,0,0)$ and $P_1, P_2, P_3$ where coordinates are unknown. However we know distances between $P_1, P_2, P_3$ (let's name them $d_{12}, d_{23}, d_{13}$) and unit vectors $v_1, v_2, v_3$, corresponding to the vectors $\overrightarrow{{P_0}{P_1}}, \overrightarrow{{P_0}{P_2}}, \overrightarrow{{P_0}{P_3}}$.

How to find coordinates of $P_1, P_2, P_3$?

I suppose (from direct analysis of geometric construction) there is not unique solution for this problem, but solution always exists... Of course we can construct three equations for this problem in the form:

$\Vert{k_1v_1- k_2v_2}\Vert= d_{12}$
$\Vert{k_1v_1- k_3v_3}\Vert= d_{13}$
$\Vert{k_2v_2- k_3v_3}\Vert= d_{23}$

which can be written also as:

${(k_1v_1- k_2v_2)}^T{(k_1v_1- k_2v_2)} = d_{12}^2$
${(k_1v_1- k_3v_3)}^T{(k_1v_1- k_3v_3)} = d_{13}^2$
${(k_2v_2- k_3v_3)}^T{(k_2v_2- k_3v_3)} = d_{23}^2$

where we have three unknown scalar coefficients $k_1, k_2,k_3$ but this set of equations is unfortunately not linear, though highly symmetrical (additionally since we know unit vectors $v_1, v_2, v_3$ we also know cosines between them $c_{12}, c_{13}, c_{23}$). Maybe this symmetry can be somehow used in the solution ? ..

2 hours later
When we introduce into system quasi-projection matrices $$ M_{12}= \begin{pmatrix} 1 & 0 & 0 \\ 0 & -1 & 0 \\ 0 & 0 & 0 \\ \end{pmatrix} M_{13}= \begin{pmatrix} 1 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & -1 \\ \end{pmatrix} M_{23}= \begin{pmatrix} 0 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & -1 \\ \end{pmatrix} $$
and $ V= \begin{pmatrix} v_1 & v_2 & v_3 \end{pmatrix}, k= \begin{pmatrix} k_1 \\ k_2 \\ k_3 \\ \end{pmatrix} $ then equations can be written as

$ \dfrac{1}{d_{12}^2}({VM_{12}k})^T({VM_{12}k})= k^T(\dfrac{1}{d_{12}^2}M_{12}V^TVM_{12})k=1$
$\dfrac{1}{d_{13}^2}({VM_{13}k})^T({VM_{13}k}) =k^T(\dfrac{1}{d_{13}^2}M_{13}V^TVM_{13})k =1$
$\dfrac{1}{d_{23}^2}({VM_{23}k})^T({VM_{23}k}) =k^T(\dfrac{1}{d_{23}^2}M_{23}V^TVM_{23})k =1$
with unknown vector $k$...all in brackets is given .. we have 3 equations of the form $k^TA_ik =1$ ...but how to calculate $k$ ?

Answer 1

Geometrically, from your three squared norms equations, we see that the solution set is the intersection of three elliptic cylinders. So, I don't think this is a linear algebraic problem. You'd better use an appropriate numerical methods for solving systems of polynomial equations.

That said, if you insists on using linear algebra, there is indeed a way to reduce your system of equations to a single equation in one variable. Here I assume that $v_1,v_2,v_3$ are linearly independent. Let $V$ be the augmented matrix $[v_1|v_2|v_3]$ and let $$ 2I-V^TV=\pmatrix{a&b&f\\ b&c&d\\ f&d&e}. $$ (Actually, $a=c=e=1$ because the $v_i$s are unit vectors, but I'll keep those symbols here.) Write $x=(k_1,k_2,k_3)^T$. Then the three equations can be rewritten as $x^TAx=d_{12}^2,\ x^TBx=d_{23}^2$ and $x^TCx=d_{13}^2$, where $$ A=\pmatrix{a&b&0\\ b&c&0\\ 0&0&0}, \quad B=\pmatrix{0&0&0\\ 0&c&d\\ 0&d&e}, \quad C=\pmatrix{a&0&f\\ 0&0&0\\ f&0&e}. $$ Since $v_1,v_2,v_3$ are unit vectors, $2I-V^TV$ and in turn all its $2\times2$ principal submatrices are positive definite. Now, if we put $$ M=\pmatrix{1&0&0\\ \frac ba&\sqrt{\frac{ce-d^2}{e}}&\frac{d}{\sqrt{e}}\\ 0&0&\sqrt{e}} \ \Rightarrow\ M^{-1}=\pmatrix{1&0&0\\ -\frac ba\sqrt{\frac{e}{ce-d^2}}&\sqrt{\frac{e}{ce-d^2}}&-\frac de\sqrt{\frac{e}{ce-d^2}}\\ 0&0&\frac1{\sqrt{e}}}, $$ then $A$ and $B$ can be simultaneously diagonalised by congruence: $$ A=M\pmatrix{a&0&0\\ 0&\frac{(ac-b^2)e}{(ce-d^2)a}&0\\ 0&0&0}M^T, \quad B=M\pmatrix{0&0&0\\ 0&1&0\\ 0&0&1}M^T. $$ So, if we let $y=(y_1,y_2,y_3)^T=M^Tx$ and $\widehat{C}=M^{-1}C\left(M^{-1}\right)^T$, then your three equations can be further rewritten as \begin{align} ay_1^2+\frac{(ac-b^2)e}{(ce-d^2)a}y_2^2&=d_{12}^2,\\ y_2^2+y_3^2&=d_{23}^2,\\ y^T\widehat{C}y&=d_{13}^2. \end{align} Let $y_2=d_{23}\cos\theta$ and $y_3=d_{23}\sin\theta$. Then $$ y_1=\pm\frac1{\sqrt{a}}\sqrt{d_{12}^2-\frac{(ac-b^2)e}{(ce-d^2)a}d_{23}^2\cos^2\theta}.\tag{$\dagger$} $$ So, for each possible leading sign in this expression of $y_1$, if you substitute those expressions of $y_1,y_2,y_3$ in terms of $\theta$ into $y^T\widehat{C}y=d_{13}^2$, you get a single equation in one variable $\theta\in[0,2\pi)$, where the domain of $\theta$ is restricted to the intervals on which the square root term in $(\dagger)$ evaluates to a real number.

Answer 2

I think the problem mainly looks difficult because the solution is far from being unique. Even if you fix $P_0$ to be the origin, you can map your three points by an orthogonal transformation and get again a solution. Looking at your problem geometrically, you see that you are looking at three points on the unit sphere with prescribed distances to each other. To fix the freedom of orthogonal transformations, you can first fix $P_1$ to be the north pole $(0,0,1)$. Since all $P_i$ have unit length, specifying their distance is equivalent to specifying the inner product between the vectors. So after fixing $P_1$, you conclude that the distance $d_{12}$ specifies the intersection of a horizontal plane with the sphere. Let $z_0\in [-1,1]$ be the height of this plane (this requires to be $d_{12}$ between $0$ and $2$ which is an obvious necessary conditions for existence of a solution). Then you still have the freedom to apply an orthogonal map fixing $(0,0,1)$ which means that without loss of generality, you can assume that $P_2=(\sqrt{1-z_0^2},0,z_0)$. At this point, all the freedom of orthogonal transformations has been essentially used up, you just can apply a reflection in the plane spanned by $P_1$ and $P_2$.

Now if $z_0\neq\pm 1$ (which excludes cases that are obviously degenerate) the distances $d_{13}$ and $d_{23}$ read as inner products specify two affine planes perpendicular to $P_1$ and $P_2$, which intersect in an affine line parallel to the y-axis. Now you see the restrictions on triangle inequality popping up as the condition that this affine line intersects the unit sphere. Except for another degenerate case (in which the three points lie in a plane through the origin), this intersection will be in two points, which are related by the reflection discussed above. Choosing one of these two points as $P_3$, you get one solution to you problem, the general solution is then obtained by acting with arbitrary orthogonal transformations.