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 );
}

Picking, Mouse Projections, Screen to Ray

 The Problem:

When the user clicks on the 2D screen, how do we know in 3D coordinates where they actually clicked, so we can select the object they clicked on?

The Solution:

Obviously a simple geometric / linear algebra problem. It is commonly done by querying the graphics card for the projection, modelview, and viewport. I despise asking the card for anything, as it should be a one way pipe. (minus CUDA, which is still new).

Our special case is, if we know how we constructed the projection matrix, we can easily invert the process to convert a screen ray into world coordinates. Then we can make those world coordinates relative to our "camera" matrix, and you can then perform a ray to scene collision detection;

This is also called "picking", "selection", and has a variety of other names. I highly reccomend never using any method but your own geometric methods, since you can't rely on this type of functionality to be consistent, without ensuring your own code is.

In layman terms, "You can click things in 3D" / "What you see is what you get"

The orange dot is the actual 3D position of the Ray -> AABB intersection
(A Ray is defined by a point and a direction, a AABB is a axis aligned bounding box, so it has a minimum position and a maximum position, or a center and a half-size for each axis)

Here's some code snippet:

class matrix
{
    public:

    //+X = left, +Y = up, +Z = forward
    float Xx, Xy, Xz, Xw;    //Order from spec; Colum major 0..16. Have to transpose all the mathematics (internally)
    float Yx, Yy, Yz, Yw;
    float Zx, Zy, Zz, Zw;
    float x, y, z, w;

    ...


void mingl::matrix::frustum_get_ray( int umouse_x, int umouse_y, int window_w, int window_h, float distnear, float distfar, float fovangle, float & Rx, float & Ry, float & Rz, float & Rdx, float & Rdy, float & Rdz )
{

    float scrn_x =  (float)(2*umouse_x - window_w) / ((float)window_w);
    float scrn_y =  -(float)(2*umouse_y - window_h) / ((float)window_h);

    float td_dx, td_dy;

    float td_tan = tan( fovangle * (M_PI/360.0f) );

    if( window_w > window_h ){

        td_dx = td_tan * scrn_x * ((float)window_w/(float)window_h);
        td_dy = td_tan * scrn_y;
    }else{

        td_dx = td_tan * scrn_x;
        td_dy = td_tan * scrn_y * ((float)window_h/(float)window_w);
    }

    //Generate points on the projection viewport
    float p1[3] = { td_dx*distnear, td_dy*distnear, distnear };
    float p2[3] = { td_dx*distfar, td_dy*distfar, distfar };
    float dv[3] = { (p2[0] - p1[0]), (p2[1] - p1[1]), (p2[2] - p1[2]) };

    //Note this is a specialized "To Global" operation (because of the inversion on axes)
    //Ray does not start from center of camera.
    Rx = x - Zx * p1[2] + Xx * p1[0] + Yx * p1[1];
    Ry = y - Zy * p1[2] + Xy * p1[0] + Yy * p1[1];
    Rz = z - Zz * p1[2] + Xz * p1[0] + Yz * p1[1];

    //note the conversion.
    Rdx = -Zx * dv[2] + Xx * dv[0] + Yx * dv[1];
    Rdy = -Zy * dv[2] + Xy * dv[0] + Yy * dv[1];
    Rdz = -Zz * dv[2] + Xz * dv[0] + Yz * dv[1];
}

 From reading that code, you'll notice the projection matrix is defined by:

    //3D projection matrix via symmetrical frustum; the most common projection.
    float fov = 60.0;
    float aspect = 1.0;
    float unear = 0.125;
    float ufar = 1024.0;

    float top = tan(fov*0.00872664625997f) * unear;    //0.00872665f = pi / 360 = (pi / 180) * 0.5
    if( (window_h > 0) && (window_w > 0) ){

        if( window_w > window_h ){

            aspect = float(window_w)/float(window_h);
            projection.frustum( aspect * -top, aspect * top, -top, top, unear, ufar );
        }else{

            aspect = float(window_h)/float(window_w);
            projection.frustum( -top, top, aspect * -top,aspect * top, unear, ufar );
        }
    }


void mingl::matrix::frustum( float ul, float ur, float ub, float ut, float un, float uf )
{
    float dx = (ur-ul);
    float dy = (ut-ub);
    float dz = (uf-un);
    dx = (dx <= 0) ? 1.0 : dx;
    dy = (dy <= 0) ? 1.0 : dy;
    dz = (dz <= 0) ? 1.0 : dz;

    identity();

    Xx = (2.0*un)/dx;
    Yy = (2.0*un)/dy;
    Xz = (ur + ul)/dx;
    Yz = (ut + ub)/dy;
    Zz = -(uf + un)/dz;
    Zw = -1.0;
    z = -(2.0*uf*un)/dz;
    w = 0;

Works beautifully! Onward with my megaman project.

-Z