Takes a number of points which lie on the surface of a 3d ellipsoid and calculates center and radii of the ellipsoid

FitPoints.py

#!/usr/bin/env python
# migrated from java code:
# https://github.com/KEOpenSource/EllipsoidFit/blob/master/EllipsoidFit/src/ellipsoidFit/FitPoints.java

import numpy as np
from numpy.linalg import svd, pinv, eig
from mpl_toolkits.mplot3d import Axes3D
from matplotlib import pyplot as plt

def solveSystem(points):
	N = len(points)

	d = np.zeros((N, 9))
	for i in range(N):
		x = points[i,0]
		y = points[i,1]
		z = points[i,2]

		d[i,0] = x*x
		d[i,1] = y*y
		d[i,2] = z*z
		d[i,3] = 2*x*y
		d[i,4] = 2*x*z
		d[i,5] = 2*y*z
		d[i,6] = 2*x
		d[i,7] = 2*y
		d[i,8] = 2*z

	dtd = np.dot(d.transpose(), d)
	ones = np.ones([N])
	dtOnes = np.dot(d.transpose(), ones)

	u, s, v = svd(dtd)
	s = np.diag(s)
	dtdi = pinv(np.dot(np.dot(u,s), v))
	v = np.dot(dtdi, dtOnes)

	return v

def formAlgebraicMatrix(v):
	a = np.zeros((4,4))

	a[0,0] = v[0]
	a[0,1] = v[3]
	a[0,2] = v[4]
	a[0,3] = v[6]
	a[1,0] = v[3]
	a[1,1] = v[1]
	a[1,2] = v[5]
	a[1,3] = v[7]
	a[2,0] = v[4]
	a[2,1] = v[5]
	a[2,2] = v[2]
	a[2,3] = v[8]
	a[3,0] = v[6]
	a[3,1] = v[7]
	a[3,2] = v[8]
	a[3,3] = -1

	return a

def findCenter(a):
	subA = - a[:3, :3]
	subV =   a[ 3, :3]

	u, s, v = svd(subA)
	s = np.diag(s)
	subAi = pinv(np.dot(np.dot(u,s), v))

	return np.dot(subAi, subV)

def translateToCenter(c, a):
	t = np.eye(4)
	t[3,:3] = c

	r = np.dot(np.dot(t, a), t.transpose())
	return r

def findRadii(evals):
	return np.sqrt(1.0/evals)

center = [10, 2, 4]
r1 = 5
r2 = 87
r3 = 10
points = np.zeros((1000, 3))
for i in range(len(points)):
	s = np.abs(2*np.pi*np.random.random())
	t = np.abs(2*np.pi*np.random.random())

	points[i,0] = r1*np.cos(s) * np.cos(t) + center[0]
	points[i,1] = r2*np.cos(s) * np.sin(t) + center[1]
	points[i,2] = r3*np.sin(s) + center[2]

if False:
	fig = plt.figure()
	ax = fig.add_subplot(111, projection='3d')
	ax.scatter(points[:,0], points[:,1], points[:,2])
	plt.show()

v = solveSystem(points)
a = formAlgebraicMatrix(v)

c = findCenter(a)

print 'center:'
print c
r = translateToCenter(c, a)

subr = r[:3,:3]
subr /= -1.0 * r[3,3]
evals, evecs = eig(subr)

radii = findRadii(evals)
print 'radii:'
print radii