Trigonometry – Find Position and Rotation of Object in 3D Space Given Pan and Tilt Angle to 3 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.

Test Data

// Positions and rotations use the UnrealEngine coordinate system (Left handed Z Up)
// +Pan rotates the light clockwise, -Pan rotates counterclockwise
// Pan 0 is center
// +Tilt pitches the light downward, -Tilt pitches the light upward
// Tilt 0 is light pointing straight up

// Data I want to find
Light Position = (X=720.081099,Y=-278.892113,Z=905.002818)
Light Nodal Position = (X=720.604299,Y=-279.103379,Z=862.271543)
Light Rotation = (Pitch=-0.288133,Yaw=90.397583,Roll=-179.300499)

// Data I Know
P1 = (X=1655.150801,Y=-102.908631,Z=447.033520)
pan1 = 169.3994∘
tilt1 = -65.80042∘

P2 = (X=1655.150801,Y=-604.529018,Z=746.498843)
pan2 = 199.3925∘
tilt2 = -82.40376∘

P3 = (X=-524.568075,Y=-101.254952,Z=528.269015)
pan3 = 8.697185∘
tilt3 = -76.00046∘

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.

EDIT

Follows a python script which locates the location point as well as the needed correction in pan $(a)$ and tilt $(b)$ angles. The script follows the general lines already covered in the MATHEMATICA script.

The calculations are made for the Real Example Data and the Test Data.

import math
import random as rd
import numpy as np
from scipy.optimize import minimize


def rand(a,b):
    r = rd.random()
    return a*r + (1 - r)*b


conv = math.pi/180
p1 = [16.549, -1.031, 4.460]
p2 = [16.549, -4.798, 6.718]
p3 = [-5.314, -0.979, 5.239]
pan  = np.array([168.657, 191.614, 4.433])*conv
tilt = np.array([-67.998,-80.749,-77.318])*conv

p1 = [1655.151, -102.909, 447.034]
p2 = [1655.151, -604.529, 746.499]
p3 = [-524.568, -101.255, 528.269]
pan  = np.array([169.400, 199.393, 8.697])*conv
tilt = np.array([-65.800,-82.404,-76.000])*conv

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

def obj(pars):
    (r1, r2, r3, a, b) = pars
    l1 = np.array(p1) + np.array(q(r1, a + pan[0], b + tilt[0]))
    l2 = np.array(p2) + np.array(q(r2, a + pan[1], b + tilt[1]))
    l3 = np.array(p3) + np.array(q(r3, a + pan[2], b + tilt[2]))
    return np.linalg.norm(l1 - l2)  + np.linalg.norm(l1 - l3)  + np.linalg.norm(l2 - l3)

ntries = 10
minerror = 100000
bnds = ((0,1660),(-600,1600),(0,1600),(-math.pi,math.pi),(-math.pi,math.pi))

for k in range(ntries):
    x0 = [rand(0,1600),rand(-600,0),rand(0,1600),0,0]
    res = minimize(obj, x0, method ='SLSQP', bounds = bnds)
    if res.fun < minerror:
        minerror = res.fun
        [r1, r2, r3, a, b] = res.x
        l1 = np.array(p1) + np.array(q(r1, a + pan[0], b + tilt[0]))
        l2 = np.array(p2) + np.array(q(r2, a + pan[1], b + tilt[1]))
        l3 = np.array(p3) + np.array(q(r3, a + pan[2], b + tilt[2]))
        p = (l1 + l2 + l3)/3


np.set_printoptions(formatter={'float': '{:5.3f}'.format})
print('position = ',p)
print('error    = {:10.4f}'.format(minerror/3))
dists = ['r1', 'r2', 'r3']
print('distances:')
for i in range(3):
    print('    ',dists[i],'   = {:10.4f}'.format(res.x[i]))
angles = ['pan ','tilt']
print('corrections:')
for i in range(2):
    print('    ',angles[i],' = {:10.4f}'.format(res.x[3+i]/conv))

In red the reference points, and in blue the location point.