Least squares fit a sphere to 3D data

I didn't find this online anywhere, and I had some data I needed a least squares fit with a sphere for.

All you have to do is define:

Error = Sum( |Position[n] - Center|^2 - Radius^2 )

Then define the squared error:

Squared Error = Sum( ( |Position[n] - Center|^2 - Radius^2 )^2 )

And solve the summation using a iterative method (like newtons, below) after pulling out the summation terms.
For example, if you do: Sum( (P.x[n] - Cx)^2 ) You get (after Expand):
Sum( P.x[n]^2 - 2*P.x[n]*Cx + Cx^2 )
And you can then split up the sum:
Sum( P.x[n]^2 ) + Sum( P.x[n] ) * -2*Cx + Cx * Nelements
Note you HAVE to ultimately divide the sums by Nelements

Note that "Center" is A,B,C (3D) and I use Rsq as Radius^2.

This method is not fast, but it converges, and the way the code is written it is independent of dataset size, but you do have to compute a number of sums and products before running the algorithm.

Note this method is used to generate the equations used to compute linear and quadratic fits instantly, given you compute some sums first. I suppose it can be extended to any shape with enough working the mathematics. The next shapes are planes, capsules, maybe torii.


//
//Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data
//
//    P is a structure that has been computed with the data earlier.
//    P.npoints is the number of elements; the length of X,Y,Z are identical.
//    P's members are logically named.
//
//    X[n] is the x component of point n
//    Y[n] is the y component of point n
//    Z[n] is the z component of point n
//
//    A is the x coordiante of the sphere
//    B is the y coordiante of the sphere
//    C is the z coordiante of the sphere
//    Rsq is the radius squared of the sphere.
//
//This method should converge; maybe 5-100 iterations or more.
//
double Xn = P.Xsum/P.npoints;        //sum( X[n] )
double Xn2 = P.Xsumsq/P.npoints;    //sum( X[n]^2 )
double Xn3 = P.Xsumcube/P.npoints;    //sum( X[n]^3 )
double Yn = P.Ysum/P.npoints;        //sum( Y[n] )
double Yn2 = P.Ysumsq/P.npoints;    //sum( Y[n]^2 )
double Yn3 = P.Ysumcube/P.npoints;    //sum( Y[n]^3 )
double Zn = P.Zsum/P.npoints;        //sum( Z[n] )
double Zn2 = P.Zsumsq/P.npoints;    //sum( Z[n]^2 )
double Zn3 = P.Zsumcube/P.npoints;    //sum( Z[n]^3 )

double XY = P.XYsum/P.npoints;        //sum( X[n] * Y[n] )
double XZ = P.XZsum/P.npoints;        //sum( X[n] * Z[n] )
double YZ = P.YZsum/P.npoints;        //sum( Y[n] * Z[n] )
double X2Y = P.X2Ysum/P.npoints;    //sum( X[n]^2 * Y[n] )
double X2Z = P.X2Zsum/P.npoints;    //sum( X[n]^2 * Z[n] )
double Y2X = P.Y2Xsum/P.npoints;    //sum( Y[n]^2 * X[n] )
double Y2Z = P.Y2Zsum/P.npoints;    //sum( Y[n]^2 * Z[n] )
double Z2X = P.Z2Xsum/P.npoints;    //sum( Z[n]^2 * X[n] )
double Z2Y = P.Z2Ysum/P.npoints;    //sum( Z[n]^2 * Y[n] )

//Reduction of multiplications
double F0 = Xn2 + Yn2 + Zn2;
double F1 = 0.5*F0;
double F2 = -8.0*(Xn3 + Y2X + Z2X);
double F3 = -8.0*(X2Y + Yn3 + Z2Y);
double F4 = -8.0*(X2Z + Y2Z + Zn3);

//Set initial conditions:
A = Xn;
B = Yn;
C = Zn;

//First iteration computation:
double A2 = A*A;
double B2 = B*B;
double C2 = C*C;
double QS = A2 + B2 + C2;
double QB = - 2*(A*Xn + B*Yn + C*Zn);

//Set initial conditions:
Rsq = F0 + QB + QS;

//First iteration computation:
double Q0 = 0.5*(QS - Rsq);
double Q1 = F1 + Q0;
double Q2 = 8*( QS - Rsq + QB + F0 );
double aA,aB,aC,nA,nB,nC,dA,dB,dC;

//Iterate N times, ignore stop condition.
int n = 0;
while( n != N ){
    n++;

    //Compute denominator:
    aA = Q2 + 16*(A2 - 2*A*Xn + Xn2);
    aB = Q2 + 16*(B2 - 2*B*Yn + Yn2);
    aC = Q2 + 16*(C2 - 2*C*Zn + Zn2);
    aA = (aA == 0) ? 1.0 : aA;
    aB = (aB == 0) ? 1.0 : aB;
    aC = (aC == 0) ? 1.0 : aC;

    //Compute next iteration
    nA = A - ((F2 + 16*( B*XY + C*XZ + Xn*(-A2 - Q0) + A*(Xn2 + Q1 - C*Zn - B*Yn) ) )/aA);
    nB = B - ((F3 + 16*( A*XY + C*YZ + Yn*(-B2 - Q0) + B*(Yn2 + Q1 - A*Xn - C*Zn) ) )/aB);
    nC = C - ((F4 + 16*( A*XZ + B*YZ + Zn*(-C2 - Q0) + C*(Zn2 + Q1 - A*Xn - B*Yn) ) )/aC);

    //Check for stop condition
    dA = (nA - A);
    dB = (nB - B);
    dC = (nC - C);
    if( (dA*dA + dB*dB + dC*dC) <= Nstop ){ break; }

    //Compute next iteration's values
    A = nA;
    B = nB;
    C = nC;
    A2 = A*A;
    B2 = B*B;
    C2 = C*C;
    QS = A2 + B2 + C2;
    QB = - 2*(A*Xn + B*Yn + C*Zn);
    Rsq = F0 + QB + QS;
    Q0 = 0.5*(QS - Rsq);
    Q1 = F1 + Q0;
    Q2 = 8*( QS - Rsq + QB + F0 );
}