How can I fit a sphere to a set of 3d points with least squares method?

Question

I am trying to fit a sphere to a set of 3d points by using the method I found here: https://jekel.me/2015/Least-Squares-Sphere-Fit.

I do understand why and how the input matrix and vector are formed, but I have not understood the least squares method itself completely. (I was hoping to skip that since I only need the result). I have gotten this far:

        // simplified example: Expected result: R=1000.0, C=(0, 1000, 0)
        points = new Vector<double>[]
        {
            DenseVector.OfArray(new double[] {0.0, 0.0, 0.0, 1.0}),
            DenseVector.OfArray(new double[] {0.0, 1000.0, 1000.0, 1.0}),
            DenseVector.OfArray(new double[] {1000.0, 1000.0, 0.0, 1.0}),
            DenseVector.OfArray(new double[] {0.0, 1000.0, -1000.0, 1.0}),
        };

        Matrix<double> A = DenseMatrix.OfRowVectors(points);
        Vector<double> f = DenseVector.Create(A.RowCount, 0.0);
        foreach (var tuple in A.EnumerateRowsIndexed())
        {
            var index = tuple.Item1;
            var row = tuple.Item2;

            // Assemble the A matrix
            row[0] *= 2.0;
            row[1] *= 2.0;
            row[2] *= 2.0;
            row[3] = 1;
            A.SetRow(index, row);

            // Assemble the f matrix
            f[index] = row[0] * row[0] + row[1] * row[1] + row[2] * row[2];
        }

        var C = MultipleRegression.NormalEquations(A, f);

        // solve for the radius
        double t = (C[0] * C[0]) + (C[1] * C[1]) + (C[2] * C[2]) + C[3];
        double radius = System.Math.Sqrt(t);

        // Actual result: R=4000, C=(0, 4000, 0)

Unfortunately, the results are incorrect. I noticed that the regression examples in the math.net documentation seam to only treat curves. Did I make a mistake or is the library not suited for this kind of problem?

Answer 1

I refactored your code into something that I think is easier to follow for constructing the different matrices, but I did find where your matrix assigments got mixed up.

In your loop constructing the A matrix and the f matrix, you modify the rows first with lines like below for your assignment to the A matrix.

row[0] *= 2.0;

Then after that assignment you are doing your calculation for f

f[index] = row[0] * row[0] + row[1] * row[1] + row[2] * row[2];

but at this point in the code, row[0] represents 2 * x, and not just x and f is supposed to be x^2 + y^2 + z^2 not (2*x)^2 + (2*y)^2 + (2*z)^2 that's why your best fit values are off by a factor of 4.

The generic method after refactoring for calculating the best fit sphere is given below.

public static (double radius, double x, double y, double z) BestFitSphere(double[] xValues, double[] yValues, double[] zValues)
{
    if ((xValues.Length != yValues.Length) || (yValues.Length != zValues.Length))
        throw new ArgumentException("Array inputs for x y z all need to be the same length");
    
    var numPoints = xValues.Length;
    
    var a = new double[numPoints, 4];
    var f = new double[numPoints];
    
    for(var i = 0; i < numPoints; i++)
    {
        a[i,0] = xValues[i] * 2;
        a[i,1] = yValues[i] * 2;
        a[i,2] = zValues[i] * 2;
        a[i,3] = 1.0;
        f[i] = (xValues[i] * xValues[i]) + (yValues[i] * yValues[i]) + (zValues[i] * zValues[i]);
    }
    
    var aMatrix = Matrix<double>.Build.DenseOfArray(a);
    var fVector = Vector<double>.Build.DenseOfArray(f);
    
    var cVector = MultipleRegression.NormalEquations(aMatrix, fVector);
    // solve for the radius
    double t = (cVector[0] * cVector[0]) + (cVector[1] * cVector[1]) + (cVector[2] * cVector[2]) + cVector[3];
    double radius = System.Math.Sqrt(t);
    return (radius, cVector[0], cVector[1], cVector[2]);
}

Then using that method with your sample data looks like this

// simplified example: Expected result: R=1000.0, C=(0, 1000, 0)
var xValues = new double[]{0.0, 0.0, 1000.0, 0.0};
var yValues = new double[]{0.0, 1000.0, 1000.0, 1000.0};
var zValues = new double[]{0.0, 1000.0, 0.0, -1000.0};

var (radius, x, y, z) = BestFitSphere(xValues, yValues, zValues);
Console.WriteLine($"radius = {radius} center = ({x},{y},{z})");

The actual result returned is

radius = 1000 center = (1.0339757656912846E-28,999.9999999999998,0)

I was testing this in dotnetfiddle, and that can be viewed here https://dotnetfiddle.net/U8dWot

Answer 2

I run the python script using the numpy library (see the link in the question) from inside C#. It returns the correct result back to the app where I can process it.