Fit data points to Sphere

Documentation Plus Sample Code

Given a set of points, find the sphere that best fits the points in a least squares sense. This algorithm for a sphere follows very closely that given for a CIRCLE. The extra dimension is easily handled by extending the equations in a logical manner.

Least Squares Algorithms

Circle: Quick, easy solution to simple 2D hard iron problem
Sphere: Quick, easy solution to simple 3D hard iron problem
Ellipse: 2D Hard and Soft iron solution
Ellipsoid: 3D Hard and Soft iron solution
Precision Ellipsoid: Precision Iterative 3D Hard and Soft iron solution

Algorithm

The classic equation for a sphere centered at a,b,c with radius R is:

( x - a )² + ( y - b )² + ( z - c )²= R²

As with the circle, we express the problem as a polynomial we can work with:

A (x² + y² + z² ) + B x + C y + D z = 1

We can form the J matrix, an n-row by 4-column matrix. The i^th row of this matrix is

[(x_i² + y_i² + z_i²) , x_i , y_i , z_i ]

So, for example, if the i^th data point were at (3,2,1) then the i^th row of the J matrix would be [(3²+2²+1² ),3,2,1] = [14,3,2,1].
Now collect the coefficients A, B, C, and D into a column vector ABC, and we can write the n equations as

J ABC = K

where K is a column of ones. From here, the equations are the same as for the circle until we get to interpreting the answer.

ABC =( J^TJ )^-1 J^T K

ABC here has a fourth term:

A = ABC[0]
B = ABC[1]
C = ABC[2]
D = ABC[3]

We convert back to standard form using

a = xofs = -B/(2 A)
b = yofs = -C/(2 A)
c = zofs = -D/(2 A)
R = sqrt(4 A + B² + C² + D²)/(2 A)

Python Source Code ls_sphere.py

Inputs are three NumPy arrays for the observed x, y, and z coordinates

Output is the tuple (X,Y,Z,R) where X,Y,Z is the center and R is the radius


import numpy as np

#least squares fit to a sphere

def ls_sphere(xx,yy,zz):
   asize = np.size(xx)
   print 'Sphere input size is ' + str(asize)
   J=np.zeros((asize,4))
   ABC=np.zeros(asize)
   K=np.zeros(asize)

   for ix in range(asize):
      x=xx[ix]
      y=yy[ix]
      z=zz[ix]

      J[ix,0]=x*x + y*y + z*z
      J[ix,1]=x
      J[ix,2]=y
      J[ix,3]=z
      K[ix]=1.0

   K=K.transpose() #not required here
   JT=J.transpose()
   JTJ = np.dot(JT,J)
   InvJTJ=np.linalg.inv(JTJ)

   ABC= np.dot(InvJTJ, np.dot(JT,K))
   #If A is negative, R will be negative
   A=ABC[0]
   B=ABC[1]
   C=ABC[2]
   D=ABC[3]

   xofs=-B/(2*A)
   yofs=-C/(2*A)
   zofs=-D/(2*A)
   R=np.sqrt(4*A + B*B + C*C + D*D)/(2*A)
   if R < 0.0: R = -R
   return (xofs,yofs,zofs,R)

if __name__ == '__main__':
# Test of least squares fit to a Sphere
# Samples have random noise added to X, Y, and Z
# True center is at (40,80,120); true radius is 400

# Output:
# Sphere input size is 9
# Center at (    43.5,    79.8,   123.3) Radius:   401.2


   xyz = np.array([
[    44.5,  472.7,   14.4 ],
[    35.5,  182.9,  504.9 ],
[    36.3, -312.9,  225.8 ],
[    43.4, -263.2,  -81.7 ],
[  -150.3,   78.0, -219.2 ],
[   388.7,   74.6,  -85.4 ],
[   142.3,   80.4, -265.2 ],
[  -347.3,  -24.2,  117.9 ],
[   428.3,  184.5,  125.7 ]
        ])

   xx=xyz[:,0]
   yy=xyz[:,1]
   zz=xyz[:,2]
   ans = ls_sphere(xx,yy,zz)

   ss="Center at (%8.1f,%8.1f,%8.1f) Radius:%8.1f" % ans
   print ss