Find a the Position and Rotation of an Object in 3D Space Given the Pan and Tilt Angle to 3 Other Known Points.

Question

Goal

I want to find the rotation and position of an object in the real world given the pan/tilt or azimuth/elevation angles between the object and 3 or more other points in the room. The rotation of the object should be relative to a fixed room axis. I then want to translate this formula into python code (please feel free to skip this part in the answer, I am mainly trying to figure out the process). If you do want to include code, please do not worry about memory or time performance.

Known Information

The points A, B, and C in 3D coordinates. The pan/tilt angles between the light/observer relative to an local up and forward vector.

Background Info

Using a DMX light (a light with motors built into it), I can find the exact local pan and tilt rotation to point the light at any object in a room by manually adjusting values until the angle is correct. Because these lights use stepper motors the exact angle can be determined to point the light in the correct direction. The origin point in the room is chosen arbitrarily, and the position of the points is measured in the real world using a laser measure and can be accurate to about 10mm.

Similar problems

While researching this problem I came across a similar question which uses 2D space. My intuition was to break the problem down into 2 2D planes, but I unfortunately am not as good at math as I'd like to believe, and I have a bit of trouble reading mathematical syntax so I was not very successful.

Calculate position based on angles between three known points

What I Have Tried

To clarity, I am not very good at 3D math so I tried to solve the problem by breaking it into 2 different 2D problems I could use that find the pan and tilt angles separately

I'll spare you all my messy python code but what I did was the following:

Input:

• 2D room positions [-1, 0], [0, 1], and [1, 0]
• pan angles -90, 0, and +90

Expected Output:

• Observer point should be [0, 0]
• Observer rotation should be 0.

Process:

1. Calculate the point of intersection between the bisector of any two positions, and the circle formed by the bisector and those two positions: Half distance between 2 positions * inverse of slope between 2 positions * cotan(angle/2)

2. Calculate the 2 points intersection between all sets of 2 points and their bisector intersection.

3. Find the common point of intersection between the two sets of intersection points.

Comments

I think my approach for a point in 2D space is pretty solid, although I do receive NaN points and Inf values from time to time (probably a result of my poor understanding of the math). But I am not really sure how I can scale this up to 3D and how I can adjust the formula to account for the rotation of the object itself. Please feel free to build on my approach or discard it all together. Although I am proud of the attempt I made, I am pretty sure this is not the best way to approach this problem.

Diagram

I am very sorry about the paint image, I know it is the Comic Sans of diagrams but please bear with me.

Concerns

Given the use of this formula (creating an accurate digital twin of a lighting fixture), rotation accuracy is very important. Although a minor positional error doesn't make much of a difference over a long distance, and rotational error can vastly offset the final result. The area this is going to be used in is about 100 square meters so the rotation can have some error, but the more accurate the better.

Real Example Data

P1 = [16.54939424,-1.03074206,4.46979548]
pan1 = 168.657816434∘
tilt1 = −67.9983215076∘

P2 = [16.54939424,-4.79847669,6.71792244]
pan2 = 191.614099336∘
tilt2 = −80.7493705653∘

P3 = [-5.31398218,-0.97888645,5.2394251]
pan3 = 4.43350881209∘
tilt3 = −77.3176165408∘

Example of Setup

This is not the real venue but instead somewhat similar outdoor venue, please note the upside down lights hanging from the trusses. What I want to find is their location relative to a fixed origin point (lets say the center of the stage on the ground) and the rotation they were installed at. For NDA reasons I cannot show the actual site of the installation, sorry

Answer 1

Consider this as an extended comment.

The object position, as emanating from each point, is

$$ q_i = P_i - r_i(\cos t_i\cos p_i, \cos t_i\sin p_i, \sin t_i)\ \ \ i=1,\cdots, n $$

Here

$$ \cases{ P_i = \text{Point coordinates}\\ r_i = \text{Distance from origin to given point}\\ t_i = \text{Tilt angle (in radians)}\\ p_i = \text{Pan angle (in radians)} } $$

One method to calculate $r_i$ is by minimizing the sum of squared residuals.

$$ \{r_i\} = \arg\min_{r_1}\sum_j\sum_{i\ne j} \|q_i-q_j\|^2 $$

so we have

$$ \mathscr{E}=\sum_i\sum_{j\ne i} \|P_i-P_j+r_i\hat u_i-r_j\hat u_j\|^2 $$

and the conditions for minimum are

$$ \frac{\partial\mathscr{E} }{\partial r_i} = (P_i-P_j+r_i\hat u_i-r_j\hat u_j)\cdot\hat u_i = 0 $$

or

$$ (P_i-P_j)\cdot\hat u_i+r_i - r_j \hat u_i\cdot\hat u_j = 0 $$

Now, after solving the linear system in the unknowns $r_i$ we have as the unknown coordinates

$$ P^* = \frac 1n\sum_i (P_i -r_i^*\hat u_i) $$

EDIT

This problem is equivalent to

$$ \{r_i,o\}=\arg\min_{r_i,o}\sum_{i=1}^n\|q_i-o\|^2,\ \ \ o = (o_x,o_y,o_z) $$

with

$$ \cases{ o^* = \frac 1n\sum_{i=1}^n q_i^*\\ \sum_{j=1}^n(P_j-r_j^*\hat u_j-o^*)\cdot\hat u_i = 0 } $$

where $o$ is the object position.

The linear system to solve in this formulation is

$$ \left[\matrix{ & & &\hat u_1^T\\ & I_n& &\vdots\\ & & &\hat u_n^T\\ \hat u_1&\cdots&\hat u_n& n I_3 }\right]\left[\matrix{r_1\\ \vdots \\ r_n\\ o}\right]=\left[\matrix{P_1\cdot\hat u_1\\ \vdots\\ P_n\cdot \hat u_n\\ \sum_i P_i}\right] $$

Follows a python script implementing this procedure

import math
from scipy import linalg as la

n = 5;
conv = math.pi/180
pan = [103.21,178.58,-92.02,-0.035,68.36]
tilt = [43.06,-54.50,15.45,-56.15,-68.25]
points = [[1.33, 8.70, 9.52],
          [-5.23, 2.75,-7.98], 
          [2.91, -6.81, 6.28], 
          [8.10, 2.81, -3.87], 
          [5.12, 7.53, -9.62]]

def q(pan, tilt):
    ct = math.cos(conv*tilt)
    st = math.sin(conv*tilt)
    cp = math.cos(conv*pan)
    sp = math.sin(conv*pan)
    return [ct*cp, ct*sp, st]

hatu = []
for k in range(n):
    hatu.append(q(pan[k],tilt[k]))

def dot(u, v):
    n = len(u)
    s = 0
    for i in range(n):
        s += u[i]*v[i]
    return s


A = [[0 for x in range(n+3)] for y in range(n+3)] 

for i in range(n):
    A[i][i] = 1
for i in range(3):
    A[i+n][i+n] = n

for i in range(n):
    for j in range(3):
        A[i][n+j] = hatu[i][j]
        A[n+j][i] = A[i][n+j]
    
B = [dot(points[i],hatu[i]) for i in range(n)]

for i in range(3):
    s = 0
    for j in range(n):
        s += points[j][i]
    B.append(s)

    
solro = la.solve(A, B)
print(solro[n:n+3])

Follows a MATHEMATICA script with randomly generated data to show the procedure

n = 5;
pan = {103.21,178.58,-92.02,-0.035,68.36}/180*Pi;
tilt = {43.06,-54.50,15.45,-56.15,-68.25}/180*Pi;
points = {{1.32704, 8.70404, 9.52376}, {-5.23097, 2.75125,-7.97803}, {2.91049, -6.80956, 6.27576}, {8.09571, 2.81425, -3.86921}, {5.12395, 7.53377, -9.61743}}
q[r_, pan_, tilt_] := r {Cos[tilt] Cos[pan], Cos[tilt] Sin[pan], Sin[tilt]}

del = Table[points[[k]] - q[r[k], pan[[k]], tilt[[k]]], {k, 1, n}];
obj = Sum[Sum[(del[[i]] - del[[j]]) . (del[[i]] - del[[j]]), {i, 1, j - 1}], {j, 2, n}];
vars = Table[r[k], {k, 1, n}]
sol = Minimize[obj, vars]
targets = del /. sol[[2]];
target = Total[targets] / n;

gr0 = Graphics3D[{Red, PointSize[0.02], Point[points]}]

gr1 = {ParametricPlot3D[del[[1]], {r[1], 0, (r[1] /. sol[[2]])}, 
PlotStyle -> {Thin, Dashed}],
ParametricPlot3D[del[[2]], {r[2], 0, (r[2] /. sol[[2]])}, 
PlotStyle -> {Thin, Dashed}],
ParametricPlot3D[del[[3]], {r[3], 0, (r[3] /. sol[[2]])}, 
PlotStyle -> {Thin, Dashed}],
ParametricPlot3D[del[[4]], {r[4], 0, (r[4] /. sol[[2]])}, 
PlotStyle -> {Thin, Dashed}],
ParametricPlot3D[del[[5]], {r[5], 0, (r[5] /. sol[[2]])}, 
PlotStyle -> {Thin, Dashed}]};
gr2 = Graphics3D[{Black, PointSize[0.02], Point[target]}];
Show[gr0, gr1, gr2]

NOTE

The angular data was generated including a random error in the range $\pm 3^{\circ}$. The points are in red, and the object position is in black.

Answer 2

To summarize the setup, we have an object with a reference frame attached to it, that specifies the object's orientation in space with respect to a world coordinate system (the coordinate system chosen for the room). In this world coordinate system we have the coordinates of $3$ points $A,B, C$. The relationship between local object coordinates and the world coordinates are as follows

$ p = p_0 + R_0 p' $

where $p$ is the world coordinate vector, and $p'$ is the local coordinate vector. $p_0$ is the position vector of the origin of the local coordinate frame relative to the world. And $R_0$ is a rotation matrix.

We are given the pan/tilt angles of the the three known points relative to the local frame. So we can write

$ p_k = p_0 + R_0 p'_k , k = 1 , 2, 3 $

where $p_1, p_2, p_3$ are the world coordinates of points $A,B,C$. and

$p'_k = s_k \ u_k $

where $u_k$ is the following unit vector

$ u_k = ( \sin \theta_k \cos \phi_k , \sin \theta_k \sin \phi_k , \cos \theta_k ) , k = 1, 2, 3$

The angles $\theta_k$, $\phi_k$ are the tilt/pan angles and are given. As points $p_1, p_2, p_3$ are known, the distances between them are known. As the vectors $u_k$ are known, the angles between them are known. This information is sufficient to determine the distances $s_k, \ k = 1, 2, 3$, as follows.

Let $\phi_1 $ be the angle between the two vectors $u_1$ and $u_2$ , $\phi_2$ be the angle between the two vectors $u_2$ and $u_3$ , and $\phi_3$ the angle between $u_1$ and $u_3$, then from the law of cosines, we have:

$ \overline{p_1 p_2}^2 = s_1^2 + s_2^2 - 2 s_1 s_2 \cos \phi_1 $

$ \overline{p_2 p_3}^2 = s_2 ^2 + s_3^2 - 2 s_2 s_3 \cos \phi_2 $

$ \overline{p_3 p_1}^2 = s_1^2 + s_3^2 - 2 s_1 s_3 \cos \phi_3 $

Solving this system numerically, we obtain $s_1, s_2, s_3$.

Now we have the following system

$ p_1 = p_0 + s_1 R_0 u_1 $

$ p_2 = p_0 + s_2 R_0 u_2 $

$ p_3 = p_0 + s_3 R_0 u_3 $

So that

$ p_1 - p_2 = R_0 (s_1 u_1 - s_2 u_2) $

$ p_1 - p_3 = R_0 (s_1 u_1 - s_3 u_3) $

Further, using the cross product on both sides, we get

$ (p_1 - p_2) \times (p_1 - p_3) = R_0 \left( (s_1 u_1 - s_2 u_2) \times (s_1 u_1 - s_3 u_3 ) \right) $

Now define the matrices $F$ and $G$ as follows:

$ F = \begin{bmatrix} p_1 - p_2 && p_1 - p_3 && (p_1 - p_2) \times (p_1 - p_3) \end{bmatrix} $

$G = \begin{bmatrix} (s_1 u_1 - s_2 u_2 ) && (s_1 u_1 - s_3 u_3) && (s_1 u_1 - s_2 u_2) \times (s_1 u_1 - s_3 u_3 ) \end{bmatrix} $

Then,

$ F = R_0 G $

Therefore,

$ R_0 = F G^{-1} $

Now that we have obtained the rotation matrix $R_0$ which specifies the orientation of the object with respect to the world, we finally solve for the position of the object using one of the position equations above, for example,

$p_1 = p_0 + s_1 R_0 u_1 $

The only unknown vector here is $p_0$, all the other quantities are known, hence,

$ p_0 = p_1 - s_1 R_0 u_1 $

Now the specification of the object's position and orientation is complete.