最小二乘法球面拟合(附完整代码)

一、不经过给定起点与终点

  文章《利用最小二乘法拟合空间圆(球)》中，最小二乘法拟合球面的方法相当巧妙、简洁。关键步骤如下：
  球面的方程为:
$$(x-a{)}^2+(y-b{)}^2+(z-c{)}^2=r^2\text{\hspace{1em}(1)}$$
  其中，$a,b,c,r\in R,r>0$。
  将(1)式展开并整理得到:
$$-2xa-2yb-2zc+1\ast (a^2+b^2+c^2-r^2)=-x^2-y^2-z^2\text{\hspace{1em}(2)}$$
  令$d=a^2+b^2+c^2-r^2$，得到关于待求参数$a,b,c,d$的线性方程：
$$-2xa-2yb-2zc+d=-x^2-y^2-z^2\text{\hspace{1em}(3)}$$
  对于给定的一系列三维数据$(x_i,y_i,z_i),i=0,...,n$，根据式(3)可以列出$n+1$个线性方程，然后可以采用最小二乘法轻而易举求解。这里通过参数代换，巧妙地将非线性的问题，转化为线性问题！

二、精确经过给定起点与终点

  有时候我们需要约束球面精确经过给定起点与终点，设起点坐标为$(x_0,y_0,z_0)$，终点坐标为$(x_n,y_n,z_n)$，则有约束等式:
$$\{\begin{array}{l}-2x_{0}a-2y_{0}b-2z_{0}c+d=-x_0^2-y_0^2-z_0^2\\ -2x_{n}a-2y_{n}b-2z_{n}c+d=-x_n^2-y_n^2-z_n^2\end{array}\text{\hspace{1em}(4)}$$
  (1)若起点与终点坐标重合，则式(4)退化为一个约束等式，将参数$d$的表达式代入到式(3)，利用最小二乘法求解参数$a,b,c$，再计算参数$d$。
  (2)若$x_0\ne x_n$，根据式(4)解出参数$a,d$的表达式，然后代入到式(3)，利用最小二乘法求解参数$b,c$，再计算参数$a,d$。
  (3)若$y_0\ne y_n$，根据式(4)解出参数$b,d$的表达式，然后代入到式(3)，利用最小二乘法求解参数$a,c$，再计算参数$b,d$。
  (4)若$z_0\ne z_n$，根据式(4)解出参数$c,d$的表达式，然后代入到式(3)，利用最小二乘法求解参数$a,b$，再计算参数$c,d$。

  这里给出完整的matlab代码:
  sphere_fitting.m

function [ center, r, fittingError ] = sphere_fitting( points, pointCount, crossStartAndEndPointFlag )
%{
Function: sphere_fitting
Description: 球面拟合
Input: 三维点points, 点个数pointCount, 是否经过起点/终点标志位crossStartAndEndPointFlag
Output: 球心center, 半径r, 拟合误差fittingError
Author: Marc Pony(marc_pony@163.com)
球面方程:(x - a)^2 + (y - b)^2  + (z - c)^2 = r^2  ->  -2*x*a - 2*y*b - 2*z*c +
         1*(a^2 + b^2 + c^2 - r^2) = -x^2 - y^2 - z^2
%}
if crossStartAndEndPointFlag == 0
    x = points(:, 1);
    y = points(:, 2);
    z = points(:, 3);
    A = [-2 * x, -2 * y, -2 * z, ones(size(x))];
    B = -x.^2 - y.^2 - z.^2;
    temp = A \ B;
    
    a = temp(1);
    b = temp(2);
    c = temp(3);
    d = temp(4);
else
    x0 = points(1, 1);
    y0 = points(1, 2);
    z0 = points(1, 3);
    xn = points(pointCount, 1);
    yn = points(pointCount, 2);
    zn = points(pointCount, 3);
    x = points(2 : pointCount - 1, 1);
    y = points(2 : pointCount - 1, 2);
    z = points(2 : pointCount - 1, 3);
    EPS = 1.0e-4;

    if abs(x0 - xn) < EPS && abs(y0 - yn) < EPS && abs(z0 - zn) < EPS %起点与终点重合
        A = [2 * (x0 - x), 2 * (y0 - y), 2 * (z0 - z)];
        B = x0^2 + y0^2 + z0^2 - x.^2 - y.^2 - z.^2 ;
        temp = A \ B;
        a = temp(1);
        b = temp(2);
        c = temp(3);
        d = -x0^2 - y0^2 - z0^2 + 2.0 * a * x0 + 2.0 * b * y0 + 2.0 * c * z0;
    else
        if abs(x0 - xn) > EPS
            A = [2 * x * (y0 - yn) / (x0 - xn) - 2 * y - 2 * (xn * y0 - x0 * yn) / (x0 - xn), 2 * x * (z0 - zn) / (x0 - xn) - 2 * z - 2 * (xn * z0 - x0 * zn) / (x0 - xn)];
            B = -x.^2 - y.^2 - z.^2 - x * (xn^2 + yn^2 + zn^2 - x0^2 - y0^2 - z0^2) / (x0 - xn) + (x0 * (xn^2 + yn^2 + zn^2) - xn * (x0^2 + y0^2 + z0^2)) / (x0 - xn);
            temp = A \ B;
            b = temp(1);
            c = temp(2);
            P = -x0^2 - y0^2 - z0^2 + 2 * y0 * b + 2 * z0 * c;
            Q = -xn^2 - yn^2 - zn^2 + 2 * yn * b + 2 * zn * c;
            a = -(P - Q) / (2 * (x0 - xn));
            d = -(P * xn - Q * x0) / (x0 - xn);
        elseif abs(y0 - yn) > EPS
            A = [2 * y * (x0 - xn) / (y0 - yn) - 2 * x - 2 * (yn * x0 - y0 * xn) / (y0 - yn), 2 * y * (z0 - zn) / (y0 - yn) - 2 * z - 2 * (yn * z0 - y0 * zn) / (y0 - yn)];
            B = -y.^2 - x.^2 - z.^2 - y * (yn^2 + xn^2 + zn^2 - y0^2 - x0^2 - z0^2) / (y0 - yn) + (y0 * (yn^2 + xn^2 + zn^2) - yn * (y0^2 + x0^2 + z0^2)) / (y0 - yn);
            temp = A \ B;
            a = temp(1);
            c = temp(2);
            P = -y0^2 - x0^2 - z0^2 + 2 * x0 * a + 2 * z0 * c;
            Q = -yn^2 - xn^2 - zn^2 + 2 * xn * a + 2 * zn * c;
            b = -(P - Q) / (2 * (y0 - yn));
            d = -(P * yn - Q * y0) / (y0 - yn);
        else
            A = [2 * z * (y0 - yn) / (z0 - zn) - 2 * y - 2 * (zn * y0 - z0 * yn) / (z0 - zn), 2 * z * (x0 - xn) / (z0 - zn) - 2 * x - 2 * (zn * x0 - z0 * xn) / (z0 - zn)];
            B = -z.^2 - y.^2 - x.^2 - z * (zn^2 + yn^2 + xn^2 - z0^2 - y0^2 - x0^2) / (z0 - zn) + (z0 * (zn^2 + yn^2 + xn^2) - zn * (z0^2 + y0^2 + x0^2)) / (z0 - zn);
            temp = A \ B;
            b = temp(1);
            a = temp(2);
            P = -z0^2 - y0^2 - x0^2 + 2 * y0 * b + 2 * x0 * a;
            Q = -zn^2 - yn^2 - xn^2 + 2 * yn * b + 2 * xn * a;
            c = -(P - Q) / (2 * (z0 - zn));
            d = -(P * zn - Q * z0) / (z0 - zn);
        end
    end
end

r = sqrt(a^2 + b^2 + c^2 - d);
center(1) = a;
center(2) = b;
center(3) = c;
fittingError = sqrt((points(:, 1) - center(1)).^2 + (points(:, 2) - center(2)).^2+ (points(:, 3) - center(3)).^2) - r;

end

  test_sphere_fitting.m

clc
clear
close all

%% 生成带误差的三维点
err = 0.1;
pointCount = 100;
R = 50;
theta = 2*pi*rand(pointCount, 1);
phi = pi*rand(pointCount, 1);
deltaR = -err + 2 * err * rand(pointCount, 1);
x = (R + deltaR) .* sin(phi) .* cos(theta);
y = (R + deltaR) .* sin(phi) .* sin(theta);
z = (R + deltaR) .* cos(phi);
plot3(x, y, z, 'ro')
hold on

%% 球面最小二乘拟合
crossStartAndEndPointFlag = 0; %0:不经过给定起点与终点;  1:精确经过给定起点与终点
[ center, r, fittingError ] = sphere_fitting( [x, y, z], pointCount, crossStartAndEndPointFlag )
maxFittingError = max(abs(fittingError))
[xx, yy, zz] = sphere(50);
xx = r * xx + center(1);
yy = r * yy + center(2);
zz = r * zz + center(3);
h = surf(xx, yy, zz);
set(h, 'FaceAlpha', 0.2, 'MeshStyle', 'none')
axis equal