Documentation Plus Sample Code
Given a set of points, find the 3D ellipsoid that best fits the points in a least squares sense. This algorithm follows very closely that given for a 2D ELLIPSE. The math for finding the center and axes is nearly identical, except for the extra dimension
Least Squares Algorithms
Algorithm
An equation for an ellipsoid centered at a, b, c with axes A, B, and C is:
( x - a )2/A2 + ( y - b )2/B2 + ( z - c )2/C2 = 1
This is not the most general form, since the ellipsoid axes are parallel to the x, y, z coordinate system axes. A more general ellipsoid can be tilted, so that the bulgy parts are NOT aligned with one of the axes. In order tilt the ellipse we have to throw in xy, xz, and yz terms. We can write the equation as:
A x2 + B y2 + C z2 + D x y + E x z + F y z + G x + H y + I z = 1
or
A x2 + B y2 + C z2 + D x y + E x z + F y z + G x + H y + I z + J = 0
where J = -1
There are only nine independent parameters: three to define the center, three to define the axes, and three define the orientation.
Suppose now that we take n noisy input samples of each component x, y and z; n >= 9. Two rules are critical to follow when taking data points:
[xi2, yi2 , zi2 , xi yi , xi zi , yi zi , xi , yi , zi ]
So, for example, if the ith data point were at (3,2,1) then the ith row
of the J matrix would be [32, 22,12,3*2,3*1,2*1,3,2,1] = [9,4,1,6,3,2,3,2,1].
Now collect the coefficients
A, through I into a column vector ABC, and we can write the n equations as
J ABC = K
where K is a column of ones. The J matrix is usually not square, so we cannot take its inverse. A modification of the equation makes it more inverse-friendly. Multiply both sides by the transpose of J. This creates a square matrix. We take the inverse of that matrix and multiply both sides by it. The resulting equation is
( JTJ )-1 JTJ ABC =( JTJ )-1 JT K
ABC =( JTJ )-1 JT K
Everything on the right is known. So, we just form the matrices, take the indicated inverse, and multiply the matrices out. The output vector, ABC, has the components
A..I = ABC[0]..ABC[8]
We bring the 1 to the other side of the equation and so get
A..J = ABC[0]..ABC[9]
The last term is -1.
We are not done yet. We have an answer but it is not in an easily understandable form. From the polynomial coefficients we have to obtain the center of the 3D ellipsoid, the lengths of the 3 axes, and the orientation of the ellipsoid. The function that does this is polyToParams3D. If you looked at the corresponding function for a 2D ellipse, polyToParams, you will notice a strong similarity. The steps are all the same, we just extend them logically to 3 dimensions. Note that the accuracy in printAns3D could be improved if you scaled the x,y,z input to one instead of just the transformation matrix. That way, the offsets get scaled too.
As in the 2D case, we have to invert a matrix and find the eigenvalues and eigenvectors of a related matrix in order to obtain the user-friendly description of the 3D ellipsoid. These extra steps are eliminated in the iterative solutions I describe later.
Python Source Code ls_ellipsoid.py
Inputs are three NumPy arrays for the observed x, y, and z coordinates
Output is a vector of the polynomial coefficients. The listing includes two sets of test data. The first is noise free, the second has added noise.
import numpy as np
from numpy.linalg import eig, inv
#least squares fit to a 3D-ellipsoid
# Ax^2 + By^2 + Cz^2 + Dxy + Exz + Fyz + Gx + Hy + Iz = 1
#
# Note that sometimes it is expressed as a solution to
# Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1
# where the last six terms have a factor of 2 in them
# This is in anticipation of forming a matrix with the polynomial coefficients.
# Those terms with factors of 2 are all off diagonal elements. These contribute
# two terms when multiplied out (symmetric) so would need to be divided by two
def ls_ellipsoid(xx,yy,zz):
# change xx from vector of length N to Nx1 matrix so we can use hstack
x = xx[:,np.newaxis]
y = yy[:,np.newaxis]
z = zz[:,np.newaxis]
# Ax^2 + By^2 + Cz^2 + Dxy + Exz + Fyz + Gx + Hy + Iz = 1
J = np.hstack((x*x,y*y,z*z,x*y,x*z,y*z, x, y, z))
K = np.ones_like(x) #column of ones
#np.hstack performs a loop over all samples and creates
#a row in J for each x,y,z sample:
# J[ix,0] = x[ix]*x[ix]
# J[ix,1] = y[ix]*y[ix]
# etc.
JT=J.transpose()
JTJ = np.dot(JT,J)
InvJTJ=np.linalg.inv(JTJ);
ABC= np.dot(InvJTJ, np.dot(JT,K))
# Rearrange, move the 1 to the other side
# Ax^2 + By^2 + Cz^2 + Dxy + Exz + Fyz + Gx + Hy + Iz - 1 = 0
# or
# Ax^2 + By^2 + Cz^2 + Dxy + Exz + Fyz + Gx + Hy + Iz + J = 0
# where J = -1
eansa=np.append(ABC,-1)
return (eansa)
Helper functions to convert the polynomial output into user-friendly parameters are below. The first is polyToParams3D. It takes the polynomial output and turns it into user-understandable parameters: the center, axes, and the rotation matrix. The second function is printAns3D. It presents the parameters in useful form. It also shows you how to use the rotation matrix to check your answer.
The output for the noisy data set is in the final listing
def polyToParams3D(vec,printMe):
# convert the polynomial form of the 3D-ellipsoid to parameters
# center, axes, and transformation matrix
# vec is the vector whose elements are the polynomial
# coefficients A..J
# returns (center, axes, rotation matrix)
#Algebraic form: X.T * Amat * X --> polynomial form
if printMe: print '\npolynomial\n',vec
Amat=np.array(
[
[ vec[0], vec[3]/2.0, vec[4]/2.0, vec[6]/2.0 ],
[ vec[3]/2.0, vec[1], vec[5]/2.0, vec[7]/2.0 ],
[ vec[4]/2.0, vec[5]/2.0, vec[2], vec[8]/2.0 ],
[ vec[6]/2.0, vec[7]/2.0, vec[8]/2.0, vec[9] ]
])
if printMe: print '\nAlgebraic form of polynomial\n',Amat
#See B.Bartoni, Preprint SMU-HEP-10-14 Multi-dimensional Ellipsoidal Fitting
# equation 20 for the following method for finding the center
A3=Amat[0:3,0:3]
A3inv=inv(A3)
ofs=vec[6:9]/2.0
center=-np.dot(A3inv,ofs)
if printMe: print '\nCenter at:',center
# Center the ellipsoid at the origin
Tofs=np.eye(4)
Tofs[3,0:3]=center
R = np.dot(Tofs,np.dot(Amat,Tofs.T))
if printMe: print '\nAlgebraic form translated to center\n',R,'\n'
R3=R[0:3,0:3]
R3test=R3/R3[0,0]
print 'normed \n',R3test
s1=-R[3, 3]
R3S=R3/s1
(el,ec)=eig(R3S)
recip=1.0/np.abs(el)
axes=np.sqrt(recip)
if printMe: print '\nAxes are\n',axes ,'\n'
inve=inv(ec) #inverse is actually the transpose here
if printMe: print '\nRotation matrix\n',inve
return (center,axes,inve)
def printAns3D(center,axes,R,xin,yin,zin,verbose):
print "\nCenter at %10.4f,%10.4f,%10.4f" % (center[0],center[1],center[2])
print "Axes gains %10.4f,%10.4f,%10.4f " % (axes[0],axes[1],axes[2])
print "Rotation Matrix\n%10.5f,%10.5f,%10.5f\n%10.5f,%10.5f,%10.5f\n%10.5f,%10.5f,%10.5f" % (
R[0,0],R[0,1],R[0,2],R[1,0],R[1,1],R[1,2],R[2,0],R[2,1],R[2,2])
# Check solution
# Convert to unit sphere centered at origin
# 1) Subtract off center
# 2) Rotate points so bulges are aligned with axes (no xy,xz,yz terms)
# 3) Scale the points by the inverse of the axes gains
# 4) Back rotate
# Rotations and gains are collected into single transformation matrix M
# subtract the offset so ellipsoid is centered at origin
xc=xin-center[0]
yc=yin-center[1]
zc=zin-center[2]
# create transformation matrix
L = np.diag([1/axes[0],1/axes[1],1/axes[2]])
M=np.dot(R.T,np.dot(L,R))
print '\nTransformation Matrix\n',M
# apply the transformation matrix
[xm,ym,zm]=np.dot(M,[xc,yc,zc])
# Calculate distance from origin for each point (ideal = 1.0)
rm = np.sqrt(xm*xm + ym*ym + zm*zm)
print "\nAverage Radius %10.4f (truth is 1.0)" % (np.mean(rm))
print "Stdev of Radius %10.4f\n " % (np.std(rm))
return
================================
Polynomial Coeffiecient for data with 5% noise on each sample:
[-0.03015596 -0.01257319 -0.02142947 0.00193205 -0.00464594 0.00161404
0.19648672 0.0883389 0.22218343 -1. ]
normed
[[ 1. -0.0320343 0.0770318 ]
[-0.0320343 0.41693894 -0.02676158]
[ 0.0770318 -0.02676158 0.71062129]]
Center at 3.0020, 4.0653, 5.0117
Axes gains 1.0072, 1.2233, 1.5833
Rotation Matrix
0.96752, -0.06211, 0.24503
-0.24890, -0.06490, 0.96635
0.04412, 0.99596, 0.07825
Transformation Matrix
[[ 0.98125763 -0.01870408 0.04093005]
[-0.01870408 0.63376398 -0.01715459]
[ 0.04093005 -0.01715459 0.82686335]]
Average Radius 0.9994 (truth is 1.0)
Stdev of Radius 0.0356