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 ) ^{2} + ( y - b )^{2} + ( z - c )^{2}= R^{2} **

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

**A (x ^{2} + y^{2} + z^{2} ) + 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}^{2} + y_{i}^{2} + z_{i}^{2}) ,
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}+2^{2}+1^{2} ),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 ^{T}J )^{-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**

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

©2022 Tom Judd

www.juddzone.com

Carlsbad, CA

USA