2
$\begingroup$

I have three 3D unit vectors $v_i =(x_i,y_i,z_i), i=1,2,3$, described relative to and passing through the origin of a Local Coordinate System:

$$ v_1 = (0.2802, -0.3965, 0.8742) \\ v_2 = (0.0986, 0.5130, 0.8527) \\ v_3 = (0.6230, 0.2279, 0.7482) $$

I have three 3D points in the Global Coordinate System:

$$ P_1 = (798.9, 29.0, -0.4) \\ P_2 = (49.8, 584.1, 1.1) \\ P_3 = (47.1, 32.4, -1.4) $$

How can I compute the rotation matrix of my Local Coordinate System to orient and position it so that Unit Vector $v_1$ passes through point $P_1$, Unit Vector $v_2$ passes through point $P_2$, and Unit Vector $v_3$ passes through point $P_3$?

The answer (which I have as this is model data) of the LCS origin X,Y,Z location, with coordinate system Roll, Pitch Yaw; and the Rotation Matrix for the above unit vectors and corresponding points is:

$LCS_{origin} =(212.92,434.94,833.85)\\ LCS_{rot} =(162.99, -11.01, 112.49) $

$ LCS Rotation Matrix_i, i=0:8 = (-0.3755, -0.8621, 0.3401, -0.9069, 0.4174, 0.0568, -0.1909, -0.2871, -0.9387)$


I believe that these 6 pieces of information (3 points and 3 unit vectors) will allow the LCS position and orientation to be “initialized” (full 3x3 rotation matrix), which I will call "data Frame 1".

Then as the LCS is moving in the GCS and the purpose of this is to characterize the LCS dynamics, I will make another unit vector measurement (relative to and through the LCS origin). $$v_4=(0.5520,0.0567,0.8319)$$ to 3D point 4 in the GCS $$P_4=(247.7,32.4,-1.4)$$ This new LCS rotation matrix I will call this "data Frame 2".

As my experiment stands now, after measuring the unit vector through $P_4$ I start a new cycle at $P_1$ and progress through my other 3D points by taking a new measurement back to 3D point 1 $$v_{1(2)}=(0.3066,-0.3827,0.8715)$$ This new LCS rotation matrix I will call this "data Frame 3".

Then measure again back to 3D point 2 $$v_{2(2)}=(0.0378,0.5103,0.8591)$$ This new LCS rotation matrix I will call this "data Frame 4"

and so on (data Frame 5)...

The "simplest" way to get subsequent "data frames" may be to be to just use the same method over and over again by adding the newest measurement, and dropping off the earliest to keep the tetrahedron proof:

$v_1, v_2, v_3$; to $P_1, P_2, P_3$

then $v_2, v_3, v_4$ to $P_2, P_3, P_4$;

then $v_3, v_4,$ second measurement of $v_{1(2)}$; to $P_3, P_4, P_1$

then $v_4,$ second measurement of $v_{1(2)}, v_{2(2)}$ to $P_4, P_1, P_2$ ...

The last part of my question is to come up with a way to apportion the "error" from rounding and/or LCS dynamics. Three measurements cannot result in a perfect fit of a rotation matrix. As measurements proceed serially in a 1,2,3 sequence, the most recent measurement (3) will be the most correct one, so I imagine I attribute an exact fit to that third (3rd) leg of the tetrahedron, then closest solution to the 2nd, the worst fit to the 1st. I would keep the unit vector directions as true measurements, and look for "nearest to" intersections between each unit vector and it's 3D point partner. Likely the shortest perpendicular line from the specific unit vector to it's partnered 3D point.

$\endgroup$
10
  • $\begingroup$ yes! please. double thanks. $\endgroup$
    – Je2b
    Mar 22, 2020 at 17:20
  • $\begingroup$ It appears to be my question in an intelligible format. thank you. Very excited to make some progress on this! $\endgroup$
    – Je2b
    Mar 22, 2020 at 17:38
  • $\begingroup$ I'll prepare an answer suitable for programmatic implementation, since that seems to be your goal. The crux of the problem is determining a fourth vertex of a tetrahedron $Q$ with opposite face $P_1P_2P_3$ which realizes the same angles as the three unit vectors. One then easily "positions" the local origin at $Q$ and accordingly "rotates" the unit vectors to align with the three edges of the tetrahedron that meet at $Q$. $\endgroup$
    – hardmath
    Mar 22, 2020 at 21:03
  • $\begingroup$ Yes I want to implement the solution in a program. I measure unit vectors: 1,2,3....n as serial observations, so the 6DoF output will be continually updated. In the example "frame" of data (in the question body), rounding the values will force a "nearest fit" but in the real world application the most recent unit vector measured is the most "correct" and the 6DoF solution will be "weighted" toward this most recent observation. This is because the LCS is moving as it is describing a rigid body that has the ability to measure unit vector direction. $\endgroup$
    – Je2b
    Mar 22, 2020 at 21:21
  • 1
    $\begingroup$ It looks like two problems then. Solving for 2 systems of three quadratic equations for: the edge lengths; and the vertex. As long as the condiotions are not too extreme. $\endgroup$
    – Je2b
    Mar 27, 2020 at 18:37

1 Answer 1

1
$\begingroup$

The problem setup involves a known triangle $P_1P_2P_3$ in $\mathbb R^3$ and tetrahedral vertex angles $\theta_1,\theta_2,\theta_3$ we want to form by positioning a fourth point $Q$ "over" the triangle. The goal is to determine a position for that fourth point $Q$.

First note the non-uniqueness of the solution in geometric terms. If $\overline Q$ is the reflection of $Q$ in the plane of triangle $P_1P_2P_3$, then $\overline Q$ will also be a solution whenever $Q$ is. This might not be an impediment for an application in which the point $Q$ is known to be only "above" or "below" the plane of the triangle; an aerial camera flying over a triangle on the ground would exclude possible locations under the ground.

Next we assume the correspondence of angles $\theta_i$ to opposite edges of the triangle is known. Comments above allude to a more complicated situation, in which the correspondence may be heuristically determined by a history of observations. But we attempt to solve the problem when the correspondence is known, merely noting that if the angles $\theta_i$ were to become equal at some point in time, historical continuity of the correspondence could be jeopardized.

Finding lengths of edges

With those caveats given, let's set up a numerical method to find the unknown lengths of the tetrahedral edges. With the correspondence that the rotated and translated image of unit vector $u_i$ should point from $Q$ to $P_i$, we make the following conventions regarding the vertex angles opposing the sides of the triangle. Let $\{i,j,k\}=\{1,2,3\}$; then:

  • known angle $\angle P_i Q P_j$ is $\theta_k \in (0,\pi)$

  • known length of edge $P_i P_j$ is $d_k$

  • unknown length of edge $QP_k$ is $x_k$

The import of this is that the tetrahedral face $P_i Q P_j$ has angle $\theta_k$ opposite the triangle side of length $d_k$. Side lengths $x_i,x_j$ of this triangle are unknowns.

triangle face of tetrahedron

Figure 1: Typical face of tetrahedron with unknown edge lengths (click to enlarge)

As Li Li commented on your closely related Question, the Law of Cosines gives us a system of three quadratic equations for three unknowns $x_1,x_2,x_3$. For simplicity we write $c_k = \cos \theta_k$:

$$ \begin{aligned} x_1^2 + x_2^2 - d_3^2 &= 2c_3 x_1 x_2 \\ x_1^2 + x_3^2 - d_2^2 &= 2c_2 x_1 x_3 \\ x_2^2 + x_3^2 - d_1^2 &= 2c_1 x_2 x_3 \end{aligned} $$

Collecting all terms on one side, we rewrite this as a polynomial system suitable for numeric solution:

$$ \vec F(x_1,x_2,x_3) := \begin{pmatrix} x_1^2 - 2c_3 x_1 x_2 + x_2^2 - d_3^2 \\ x_1^2 - 2c_2 x_1 x_3 + x_3^2 - d_2^2 \\ x_2^2 - 2c_1 x_2 x_3 + x_3^2 - d_1^2 \\ \end{pmatrix} = \vec 0 $$

One expects, for example, with a sufficiently good initial guess and a nonsingular Jacobian of $\vec F$ that the Newton-Raphson iterations will converge rapidly to a solution. The Jacobian of $\vec F$ is a $3\times 3$ matrix with first degree polynomial entries:

$$ \nabla \vec F = \begin{pmatrix} 2x_1 - 2c_3 x_2 & -2c_3 x_1 + 2x_2 & 0 \\ 2x_1 - 2c_2 x_3 & 0 & -2c_2 x_1 + 2x_3 \\ 0 & 2x_2 - 2c_1 x_3 & -2c_1 x_2 + 2x_3 \end{pmatrix}$$

Our iterations will then take the form:

$$ \begin{pmatrix} x_1 \\ x_2 \\ x_3 \end{pmatrix} \gets \begin{pmatrix} x_1 \\ x_2 \\ x_3 \end{pmatrix} - (\nabla \vec F(x_1,x_2,x_3))^{-1} \vec F(x_1,x_2,x_3) $$

Example of finding lengths

Let's review the constants $c_k, d_k^2$ from the original problem posted. As noted above the $c_k$ are cosines of the angles between the unit vectors:

$$ \begin{aligned} c_1 &= v_2 \cdot v_3 &= 0.81633064 \\ c_2 &= v_1 \cdot v_3 &= 0.73827869 \\ c_3 &= v_1 \cdot v_2 &= 0.56965356 \end{aligned} $$

The constants $d_k^2$ are squared lengths of the sides of the triangle $P_1P_2P_3$:

$$ \begin{aligned} d_1^2 &= ||P_2-P_3||^2 &= 304,386.98 \\ d_2^2 &= ||P_1-P_3||^2 &= 565,215.80 \\ d_3^2 &= ||P_1-P_2||^2 &= 869,289.07 \end{aligned} $$

The Newton-Raphson algorithm requires us to supply an initial vector ("starting guess") for the solution. In the intended application there will presumably be a previous solution expected to be close to the next frame, which addresses that need. But in this case I used a somewhat ad hoc method to choose an initial vector.

The tetrahedron has a vertex $Q$ that we picture as lying somewhere above the triangle $P_1P_2P_3$. Thus the edge lengths $x_k$ will be greater than the distances from the points $P_k$ to the centroid of that base triangle. This suggested to me taking as initial guesses a multiple of those distances greater than one, and with a little trial and error I settled on a multiple of $1.6$. NB: With the revised coordinate for $P_2$, I kept the old initial guess, in the spirit of seeing if the solution process is robust.

I implemented the Newton-Raphson iteration in a spreadsheet, making duplicate sheets to go from one step to the next. The procedure converged reasonably well in half a dozen steps:

$$ \begin{array}{c|r|r|r|r|r|r|} \text{Step} & x_1 & x_2 & x_3 & F_1(x_1,x_2,x_3) & F_2(x_1,x_2,x_3) & F_3(x_1,x_2,x_3) \\ \hline 0 & 801.09 & 402.14 & 403.23 & -432,857.94 & -237,840.36 & -244,820.30 \\ \hline 1 & 1276.22 & 1423.32 & 1042.23 & 715,767.21 & 185,773.81 & 385,759.73 \\ \hline 2 & 1232.58 & 938.52 & 334.66 & 212,829.71 & 456,959.17 & 175,632.38 \\ \hline 3 & 1090.89 & 906.68 & 489.09 & 15,949.03 & 76,231.25 & 32,889.71 \\ \hline 4 & 1073.41 & 913.93 & 569.25 & 502.85 & 8,801.32 & 5,529.23 \\ \hline 5 & 1072.59 & 914.60 & 586.57 & 1.73 & 321.61 & 281.70 \\ \hline 6 & 1072.60 & 914.57 & 587.39 & 0.00 & 0.66 & 0.70 \\ \hline 7 & 1072.60 & 914.57 & 587.39 & 0.00 & 0.00 & 0.00 \\ \hline \end{array} $$

Solving for Q by trilateration

Once those edge lengths are found, we have a problem of finding the intersection of three spheres. In the previous sections the unknowns were the edge lengths, but now they are "knowns", so I'd like to change our notation to make the new unknowns, the coordinates of point $Q$ more prominent.

If we relabel as radii of spheres $r_k = x_k\, (k = 1,2,3)$ the edge lengths found above, equations for our three spheres take the form:

$$ ||Q-P_1|| = r_1 \\ ||Q-P_2|| = r_2 \\ ||Q-P_3|| = r_3 $$

The algebra for this system of equations is a bit easier if one of the corners of the triangle $P_1P_2P_3$ is the origin. We can arrange this to be the case by subtracting, say $P_3$, from each of the other points, including the unknown point $Q$. This rigid translation preserves distances. So with unknown point:

$$ Q - P_3 = (x,y,z) $$

we label similarly the known points:

$$ P_1 - P_3 = (f_1,g_1,h_1) \\ P_2 - P_3 = (f_2,g_2,h_2) $$

Now our equations take the form:

$$ \begin{aligned} (x-f_1)^2 + (y-g_1)^2 + (z-h_1)^2 &= r_1^2 \\ (x-x_B)^2 + (y-y_B)^2 + (z-z_B)^2 &= r_2^2 \\ x^2 + y^2 + z^2 = r_3^2 \end{aligned} $$

Subtracting each of the first two equations from the third gives a system of two linear equations:

$$ \begin{aligned} 2f_1x + 2g_1y + 2h_1z &= r_3^2 - r_1^2 + f_1^2 + g_1^2 + h_1^2 \\ 2f_2x + 2g_2y + 2h_2z &= r_3^2 - r_2^2 + f_2^2 + g_2^2 + h_2^2 \end{aligned} $$

The fact that the triangle $P_1P_2P_3$ is not degenerate implies that these two planes are not parallel (nor coincident), so they intersect in a line. Putting the above linear system in reduced row echelon form gives us (using whichever variable is without a leading one) a parametric line, say:

$$ \{(t,m_1t+b_1,m_2t+b_2)\mid t\in \mathbb R \} $$

All that remains is to solve the quadratic equation for the parameter $t$:

$$ t^2 + (m_1t+b_1)^2 + (m_2t+b_2)^2 = r_3^2 $$

which provides the intersections (if any) of the line with the third sphere (centered at the origin). Plugging the appropriate value of $t$ into the parametric line and adding $P_3$ to that point gives point $Q$.

$\endgroup$
7
  • $\begingroup$ I mis-typed the Point 2 y value in the question. The actual value is 584.1 $$P_2 = (49.8, 584.1, 1.1) $$ $\endgroup$
    – Je2b
    Mar 30, 2020 at 0:07
  • 1
    $\begingroup$ I just re-checked and all the other numbers are correct. Just the 584.1 number I entered wrong $\endgroup$
    – Je2b
    Mar 30, 2020 at 1:49
  • $\begingroup$ Correct @hardmath the cosines of the angles between the unit vectors are unchanged. It was a mistake by me in entering my data in to the post body. I corrected it in the post. instead of: $P_2(y)=84.1$ the correct value is: $P_2(y)=584.1$ Thank you and I will be more careful in the future. $\endgroup$
    – Je2b
    Mar 30, 2020 at 16:24
  • $\begingroup$ It was a worthwhile exercise, not only to see how robust the proposed Newton-Raphson solution process is, but how my spreadsheet design held up to redoing it. $\endgroup$
    – hardmath
    Mar 30, 2020 at 16:45
  • $\begingroup$ Super good news @hardmath. Now we have the three leg lengths, so we can make a sphere with radius location at each of the 3 Points, and find the 3D intersection point location in the GCS. Should we use the same Newton-Raphson process to find the solution? stackoverflow.com/questions/1406375/… $\endgroup$
    – Je2b
    Mar 30, 2020 at 17:54

You must log in to answer this question.

Not the answer you're looking for? Browse other questions tagged .