球面拟合算法

我在以前的博客中写过一篇介绍过最小二乘法圆拟合算法。前段时间有人给我留言，问我他的数据点是分布在一个球的球面上的，该如何拟合这个球的参数。球面拟合与圆的拟合其实是很类似的。只要按照我上篇文章的方法重新推导一下就能出来。正好这两天不太忙，在出差的路上推导了一下，把结果放到这里供大家参考。顺便多说一句，在高铁上用降噪耳机效果真的很好，很安静，值得拥有。

简单描述一下问题：我们有一组数据： $(x_i, y_i, z_i)$ , 这些数据是分散在一个球面上的，当然数据中混杂着一些噪声。球面方程

(x - x_{0})^{2} + (y - y_{0})^{2} + (z - z_{0})^{2} = R^{2}

$(x-x_0)^2+(y-y_0)^2+(z-z_0)^2=R^2$

其中 $(x_0, y_0, z_0, R)$ 是我们需要求出的参数。

我们构造一个函数：

H (x_{0}, y_{0}, z_{0}, R) = \sum_{i = 1}^{N} {((x_{i} - x_{0})^{2} + (y_{i} - y_{0})^{2} + (z_{i} - z_{0})^{2} - R^{2})}^{2}

$H(x_0, y_0, z_0 , R)=\sum_{i=1}^{N}\left((x_i-x_0)^2+(y_i-y_0)^2+(z_i-z_0)^2-R^2\right)^2$
我们求的

(x_{0}, y_{0}, z_{0}, R)

$(x_0, y_0, z_0, R)$ 是使 H 取得最小值的那组参数。也就是满足下面四个方程的

(x_{0}, y_{0}, z_{0}, R)

$(x_0, y_0, z_0, R)$ .

\frac{\partial H}{\partial x_{0}} = - 4 \sum_{i = 1}^{N} (x_{i} - x_{0}) ((x_{i} - x_{0})^{2} + (y_{i} - y_{0})^{2} + (z_{i} - z_{0})^{2} - R^{2}) = 0 \frac{\partial H}{\partial y_{0}} = - 4 \sum_{i = 1}^{N} (y_{i} - z_{0}) ((x_{i} - x_{0})^{2} + (y_{i} - y_{0})^{2} + (z_{i} - z_{0})^{2} - R^{2}) = 0 \frac{\partial H}{\partial z_{0}} = - 4 \sum_{i = 1}^{N} (z_{i} - z_{0}) ((x_{i} - x_{0})^{2} + (y_{i} - y_{0})^{2} + (z_{i} - z_{0})^{2} - R^{2}) = 0 \frac{\partial H}{\partial R} = - 4 \sum_{i = 1}^{N} R ((x_{i} - x_{0})^{2} + (y_{i} - y_{0})^{2} + (z_{i} - z_{0})^{2} - R^{2}) = 0

$\frac{\partial H}{\partial x_0} =-4\sum _{i=1}^N \left(x_i-x_0\right) \left(\left(x_i-x_0\right){}^2+\left(y_i-y_0\right){}^2+\left(z_i-z_0\right){}^2-R^2\right)= 0\\ \frac{\partial H}{\partial y_0} = -4\sum _{i=1}^N \left(y_i-z_0\right) \left(\left(x_i-x_0\right){}^2+\left(y_i-y_0\right){}^2+\left(z_i-z_0\right){}^2-R^2\right)=0 \\ \frac{\partial H}{\partial z_0} = -4\sum _{i=1}^N \left(z_i-z_0\right) \left(\left(x_i-x_0\right){}^2+\left(y_i-y_0\right){}^2+\left(z_i-z_0\right){}^2-R^2\right)= 0\\ \frac{\partial H}{\partial R} = -4 \sum _{i=1}^N R \left(\left(x_i-x_0\right){}^2+\left(y_i-y_0\right){}^2+\left(z_i-z_0\right){}^2-R^2\right) = 0$

最后一个方程可以化简为：

\sum_{i = 1}^{N} ((x_{i} - x_{0})^{2} + (y_{i} - y_{0})^{2} + (z_{i} - z_{0})^{2} - R^{2}) = 0

$\sum _{i=1}^N \left(\left(x_i-x_0\right){}^2+\left(y_i-y_0\right){}^2+\left(z_i-z_0\right){}^2-R^2\right) = 0$
利用这个式子可以把前三个方程化简为：

\sum_{i = 1}^{N} x_{i} ((x_{i} - x_{0})^{2} + (y_{i} - y_{0})^{2} + (z_{i} - z_{0})^{2} - R^{2}) = 0 \sum_{i = 1}^{N} y_{i} ((x_{i} - x_{0})^{2} + (y_{i} - y_{0})^{2} + (z_{i} - z_{0})^{2} - R^{2}) = 0 \sum_{i = 1}^{N} z_{i} ((x_{i} - x_{0})^{2} + (y_{i} - y_{0})^{2} + (z_{i} - z_{0})^{2} - R^{2}) = 0

$\sum _{i=1}^N x_i \left(\left(x_i-{x_0}\right){}^2+\left(y_i-{y_0}\right){}^2+\left(z_i-{z_0}\right){}^2-R^2\right)= 0\\ \sum _{i=1}^N y_i \left(\left(x_i-{x_0}\right){}^2+\left(y_i-{y_0}\right){}^2+\left(z_i-{z_0}\right){}^2-R^2\right)=0 \\ \sum _{i=1}^N z_i \left(\left(x_i-{x_0}\right){}^2+\left(y_i-{y_0}\right){}^2+\left(z_i-{z_0}\right){}^2-R^2\right)= 0\\$
为了进一步化简，再做一次变换：

u_{i} = x_{i} - \bar{x} v_{i} = y_{i} - \bar{y} w_{i} = z_{i} - \bar{z}

$u_i = x_i - \bar x \\ v_i = y_i - \bar y \\ w_i = z_i - \bar z$
这样替换之后有：

\sum_{i = 1}^{N} u_{i} ((u_{i} - u_{0})^{2} + (v_{i} - v_{0})^{2} + (w_{i} - w_{0})^{2} - R^{2}) = 0 \sum_{i = 1}^{N} v_{i} ((u_{i} - u_{0})^{2} + (v_{i} - v_{0})^{2} + (w_{i} - w_{0})^{2} - R^{2}) = 0 \sum_{i = 1}^{N} w_{i} ((u_{i} - u_{0})^{2} + (v_{i} - v_{0})^{2} + (w_{i} - w_{0})^{2} - R^{2}) = 0 \sum_{i = 1}^{N} ((u_{i} - u_{0})^{2} + (v_{i} - v_{0})^{2} + (w_{i} - w_{0})^{2} - R^{2}) = 0

$\sum _{i=1}^N u_i \left(\left(u_i-{u_0}\right){}^2+\left(v_i-{v_0}\right){}^2+\left(w_i-{w_0}\right){}^2-R^2\right)= 0\\ \sum _{i=1}^N v_i \left(\left(u_i-{u_0}\right){}^2+\left(v_i-{v_0}\right){}^2+\left(w_i-{w_0}\right){}^2-R^2\right)=0 \\ \sum _{i=1}^N w_i \left(\left(u_i-{u_0}\right){}^2+\left(v_i-{v_0}\right){}^2+\left(w_i-{w_0}\right){}^2-R^2\right)= 0\\ \sum _{i=1}^N \left(\left(u_i-{u_0}\right){}^2+\left(v_i-{v_0}\right){}^2+\left(w_i-{w_0}\right){}^2-R^2\right) = 0$

再次化简后有：

(\sum u_{i}^{2}) u_{0} + (\sum u_{i} v_{i}) v_{0} + (\sum u_{i} w_{i}) w_{0} = \frac{\sum (u_{i}^{3} + u_{i} v_{i}^{2} + u_{i} w_{i}^{2})}{2} (\sum u_{i} v_{i}) u_{0} + (\sum v_{i}^{2}) v_{0} + (\sum v_{i} w_{i}) w_{0} = \frac{\sum (u_{i}^{2} v_{i} + v_{i}^{3} + v_{i} w_{i}^{2})}{2} (\sum u_{i} w_{i}) u_{0} + (\sum v_{i} w_{i}) v_{0} + (\sum w_{i}^{2}) w_{0} = \frac{\sum (u_{i}^{2} w_{i} + v_{i}^{2} w_{i} + w_{i}^{3})}{2}

$(\sum u_i^2 )u_0 +(\sum{u_i v_i})v_0+(\sum {u_i w_i}) w_0= \frac{\sum{(u_i^3+u_i v_i^2+u_i w_i^2)}}{2} \\ (\sum u_i v_i )u_0 +(\sum{ v_i^2})v_0+(\sum {v_i w_i}) w_0= \frac{\sum{(u_i^2 v_i+ v_i^3+v_i w_i^2)}}{2} \\ (\sum u_i w_i )u_0 +(\sum{v_i w_i})v_0+(\sum {w_i^2}) w_0= \frac{\sum{(u_i^2 w_i+v_i^2 w_i+ w_i^3)}}{2}$
解出这个方程就可以得到

(u_{0}, v_{0}, w_{0})

$(u_0, v_0, w_0)$ , 然后就可以得到

(x_{0}, y_{0}, z_{0})

$(x_0, y_0, z_0)$ 了。之后带入第四个式子，就可以得到

R

$R$ 的值：

R = \sqrt{\frac{\sum_{i = 1}^{N} ((u_{i} - u_{0})^{2} + (v_{i} - v_{0})^{2} + (w_{i} - w_{0})^{2})}{N}}

$R =\sqrt{ \frac{\sum _{i=1}^N\left(\left(u_i-{u_0}\right){}^2+\left(v_i-{v_0}\right){}^2+\left(w_i-{w_0}\right){}^2\right)} {N}}$

下面是代码：

struct Point3D
{
    double x;
    double y;
    double z;
};

// 全选主元高斯消去法
//a-n*n 存放方程组的系数矩阵，返回时将被破坏
//b-常数向量
//x-返回方程组的解向量
//n-存放方程组的阶数
//返回0表示原方程组的系数矩阵奇异
int cagaus(double a[],double b[],int n,double x[])
{
    int *js, l, k, i, j, is, p, q;
    double d, t;
    js=new int [n];
    l=1;
    for (k=0;k<=n-2;k++)
    {
        d=0.0;
        for (i=k;i<=n-1;i++)
        {
            for (j=k;j<=n-1;j++)
            {
                t=fabs(a[i*n+j]);
                if (t>d)
                {
                    d=t;
                    js[k]=j;
                    is=i;
                }
            }
        }
        if (d+1.0==1.0)
        {
            l=0;
        }
        else
        {
            if (js[k]!=k)
            {
                for (i=0;i<=n-1;i++)
                {
                    p=i*n+k;
                    q=i*n+js[k];
                    t=a[p];
                    a[p]=a[q];
                    a[q]=t;
                }
            }
            if (is!=k)
            {
                for (j=k;j<=n-1;j++)
                {
                    p=k*n+j;
                    q=is*n+j;
                    t=a[p];
                    a[p]=a[q];
                    a[q]=t;
                }
                t=b[k];
                b[k]=b[is];
                b[is]=t;
            }
        }
        if (l==0)
        {
            delete js;
            return(0);
        }
        d=a[k*n+k];
        for (j=k+1;j<=n-1;j++)
        {
            p=k*n+j;
            a[p]=a[p]/d;
        }
        b[k]=b[k]/d;
        for (i=k+1;i<=n-1;i++)
        {
            for (j=k+1;j<=n-1;j++)
            {
                p=i*n+j;
                a[p]=a[p]-a[i*n+k]*a[k*n+j];
            }
            b[i]=b[i]-a[i*n+k]*b[k];
        }
    }
    d=a[(n-1)*n+n-1];
    if (fabs(d)+1.0==1.0)
    {
        delete js;
        return(0);
    }
    x[n-1]=b[n-1]/d;
    for (i=n-2;i>=0;i--)
    {
        t=0.0;
        for (j=i+1;j<=n-1;j++)
        {
            t=t+a[i*n+j]*x[j];
        }
        x[i]=b[i]-t;
    }
    js[n-1]=n-1;
    for (k=n-1;k>=0;k--)
    {
        if (js[k]!=k)
        {
            t=x[k];
            x[k]=x[js[k]];
            x[js[k]]=t;
        }
    }
    delete js;
    return 1;
}
bool sphereLeastFit(const std::vector<Point3D> &points,
                    double &center_x, double &center_y, double &center_z, double &radius)
{
    center_x = 0.0f;
    center_y = 0.0f;
    radius = 0.0f;

    int N = points.size();
    if (N < 4) // 最少 4 个点才能拟合出一个球面。
    {
        return false;
    }

    double sum_x = 0.0f, sum_y = 0.0f, sum_z = 0.0f;

    for (int i = 0; i < N; i++)
    {
        double x = points[i].x;
        double y = points[i].y;
        double z = points[i].z;
        sum_x += x;
        sum_y += y;
        sum_z += z;
    }
    double mean_x = sum_x / N;
    double mean_y = sum_y / N;
    double mean_z = sum_z / N;
    double sum_u2 = 0.0f, sum_v2 = 0.0f, sum_w2 = 0.0f;
    double sum_u3 = 0.0f, sum_v3 = 0.0f, sum_w3 = 0.0f;
    double sum_uv = 0.0f, sum_uw = 0.0f, sum_vw = 0.0f;
    double sum_u2v = 0.0f, sum_uv2 = 0.0f, sum_u2w = 0.0f, sum_v2w = 0.0f, sum_uw2 = 0.0f, sum_vw2 = 0.0f;
    for (int i = 0; i < N; i++)
    {
        double u = points[i].x - mean_x;
        double v = points[i].y - mean_y;
        double w = points[i].z - mean_z;
        double u2 = u * u;
        double v2 = v * v;
        double w2 = w * w;
        sum_u2 += u2;
        sum_v2 += v2;
        sum_w2 += w2;
        sum_u3 += u2 * u;
        sum_v3 += v2 * v;
        sum_w3 += w2 * w;
        sum_uv += u * v;
        sum_vw += v * w;
        sum_uw += u * w;
        sum_u2v += u2 * v;
        sum_u2w += u2 * w;
        sum_uv2 += u * v2;
        sum_v2w += v2 * w;
        sum_uw2 += u * w2;
        sum_vw2 += v * w2;
    }

    double A[3][3];
    double B[3] = {(sum_u3 + sum_uv2 + sum_uw2) / 2.0,
                   (sum_u2v + sum_v3 + sum_vw2) / 2.0,
                   (sum_u2w + sum_v2w + sum_w3) / 2.0};

    A[0][0] = sum_u2;
    A[0][1] = sum_uv;
    A[0][2] = sum_uw;
    A[1][0] = sum_uv;
    A[1][1] = sum_v2;
    A[1][2] = sum_vw;
    A[2][0] = sum_uw;
    A[2][1] = sum_vw;
    A[2][2] = sum_w2;

    double ans[3];

    if( cagaus((double *) A, B, 3, ans) == 0)
    {
        return false;
    }
    else
    {
        center_x = ans[0] + mean_x;
        center_y = ans[1] + mean_y;
        center_z = ans[2] + mean_z;

        double sum = 0;
        for(int i = 0; i < N; i++)
        {
            double dx = points[i].x - center_x;
            double dy = points[i].y - center_y;
            double dz = points[i].z - center_z;
            sum += dx * dx + dy * dy + dz * dz;
        }
        sum /= N;
        radius = sqrt(sum);
    }
    return true;
}

球面拟合算法

球面拟合算法

猜你喜欢