Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
import numpy as np
def ellipsoidFit(X, dtype=np.float32):
X = np.array(X)
N = X.shape[0]
mu = np.mean(X, axis=0)
for i in range(N):
X[i,:] = X[i,:] - mu
if N < 9:
raise ValueError('Need at least 9 points to fit an ellipsoid')
x = X[:,0]
y = X[:,1]
z = X[:,2]
A = np.empty((N,9), dtype=dtype)
A[:,0] = x**2 + y**2 - 2*z*z
A[:,1] = x**2 + z**2 - 2*y*y
A[:,2] = 2*x*y
A[:,3] = 2*x*z
A[:,4] = 2*y*z
A[:,5] = 2*x
A[:,6] = 2*y
A[:,7] = 2*z
A[:,8] = 1
b = x**2 + y**2 + z**2
u = np.linalg.solve(np.matmul(A.T, A), np.matmul(A.T, b))
v = np.zeros(10)
v[0] = u[0] + u[1] - 1
v[1] = u[0] - 2*u[1] - 1
v[2] = u[1] - 2*u[0] - 1
v[3] = u[2]
v[4] = u[3]
v[5] = u[4]
v[6] = u[5]
v[7] = u[6]
v[8] = u[7]
v[9] = u[8]
B = np.empty((4,4), dtype=dtype);
B = np.array([[v[0], v[3], v[4], v[6]],
[v[3], v[1], v[5], v[7]],
[v[4], v[5], v[2], v[8]],
[v[6], v[7], v[8], v[9]]])
center = np.linalg.solve(-B[:3,:3], B[:3,3])
center = center + mu
evals, evecs = np.linalg.eig(B[:3,:3]*(1./v[9]))
radii = np.sqrt(1./ abs(evals))
return v[:6]*(1./v[9]), radii, center