Geometry - Calculate position and orientation of 3D object by intersection lines from focus

Question

• I have a 3D object with its reference system at the origin O2 (I know the position of all points of the object with respect to O2), so I know the coordinates of points P1, P2, P3, P4 with respect to O2.
• At the same time, I have another reference system with the origin O1.
• The 3D object is located in some unknown position and orientation within the space of O1 (O1O2 is unknown).
• I know the coordinates of points Q1, Q2, Q3, Q4 with respect to O1.
• Qi and Pi are on the same line with the parametric equation ri(t) = O1 + ti*(Qi - O1).

How can I calculate the position of P1, P2, P3, P4 with respect to O1?

Answer 1

From the equation provided in the question, you have

$ r_i(t) = O_1 + t_i (Q_i - O_1) $

And $P_i$ lies on this line, so

$ P_i = O_1 + t_i (Q_i - O_1) $

The vectors $Q_i$ and $O_1$ are known, while the $P_i$'s are unknown. However we know the relative coordinates of the $P_i's$ with respect to a coordinate frame whose origin is at a unknown point $O_2$ and we don't know its orientation with respect to the world (it is unspecified).

However from this frame we can calculate the distances $\overline{P_1P_2}$, $\overline{P_1P_3}$, $\overline{P_2P_3}$.

So now it follows from the above equation that

$ \overline{P_i P_j} = | t_i (Q_i - O_1) - t_j (Q_j - O_1) | $

where $t_i, t_j \gt 0 $

Using the known distances $\overline{P_1P_2}$, $\overline{P_1P_3}$, $\overline{P_2P_3}$ we now have a system of three quadratic equations in three unknowns $t_1, t_2, t_3$. Solving the system gives us $P_1, P_2, P_3$.

Now the coordinates of $P_1, P_2, P_3$ with respect the object's frame whose origin is at $O_2$ are known, so we can write

$ P_i = O_2 + R_o P_i' , i = 1,2,3 $

here the vectors $P_i'$ are the coordinates of $P_i$ with resepct to the object frame, and are known. However, $O_2$ is unknown. And $R_o$ is unknown.

Since we have computed $P_i$ in the world coordinates system, we now take differnces $P_i P_j$, to obtain

$ P_i P_j = R_o (P_i' P_j') $

So that,

$[ P_1 P_2 , P_1 P_3, P_2 P_3 ] = R_o [ P_1'P_2' , P_1'P_3', P_2'P_3'] $

However, we cannot determine $R_o$ from this matrix equation because

$ P_2' P_3' = P_3' P_1' - P_1' P_2' $

Therefore, the matirx on the right hand side is singular. To overcome this, we use the cross product, and use the fact that for a rotation matrix

$ (R V) \times (R W) = R (V \times W) $

So we'll compute

$ V = P_1 P_2 \times P_1 P_3 $

as well as

$ V' = P_1' P_2' \times P_1' P_3' $

And we'll replace the above matrix equation with

$[ P_1 P_2 , P_1 P_3, V ] = R_o [ P_1'P_2' , P_1'P_3', V'] $

so that,

$ R_o = [ P_1 P_2 , P_1 P_3, V ][ P_1'P_2' , P_1'P_3', V']^{-1} $

and from $R_o$ and one the the three equations involving $O_2$, the position vector $O_2$ can be computed. And thus we have found the position $O_2$ of the object, as well as its orientation specified by $R_o$.