问题

现有近似分布在球面上的点云,试进行球面拟合,求出球心坐标和半径。

球面方程为:

$$ (x-x_0)^{2}+(y-y_0)^{2}+(z-z_0)^{2}=r^{2} $$

其中 $ x_0, y_0, z_0, r $是我们要求的参数。

利用求偏导的方式求最小值

构造方程:

$$ E(x_0,y_0,z_0,r)=\sum_{i=0}^n((x-x_0)^2+(y-y_0)^2+(z-z_0)^2-r^2)^2 $$

使方程 $E$ 值最小的参数 $x_0, y_0, z_0, r$ 就是我们的所求。
求偏导并使其等于0:

$$ \frac{\partial E}{\partial x_0}=-4\sum_{i=0}^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 E}{\partial y_0}=-4\sum_{i=0}^n(x_i-y_0)((x_i-x_0)^2+(y_i-y_0)^2+(z_i-z_0)^2-r^2)=0 $$

$$ \frac{\partial E}{\partial z_0}=-4\sum_{i=0}^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 E}{\partial r}=-4\sum_{i=0}^nr((x_i-x_0)^2+(y_i-y_0)^2+(z_i-z_0)^2-r^2)=0 $$

化简得:

$$ \sum_{i=0}^nx_i((x_i-x_0)^2+(y_i-y_0)^2+(z_i-z_0)^2-r^2)=0 $$

$$ \sum_{i=0}^ny_i((x_i-x_0)^2+(y_i-y_0)^2+(z_i-z_0)^2-r^2)=0 $$

$$ \sum_{i=0}^nz_i((x_i-x_0)^2+(y_i-y_0)^2+(z_i-z_0)^2-r^2)=0 $$

$$ \sum_{i=0}^n((x_i-x_0)^2+(y_i-y_0)^2+(z_i-z_0)^2-r^2)=0 $$

整理得:

$$ \frac{\overline{x^3}}{\overline{x}}-2x_0\frac{\overline{x^2}}{\overline{x}}+x_0^2+\frac{\overline{xy^2}}{\overline{x}}-2y_0\frac{\overline{xy}}{\overline{x}}+y_0^2+\frac{\overline{xz^2}}{\overline{x}}-2z_0\frac{\overline{xz}}{\overline{x}}+z_0^2=r^2 \tag{1} $$

$$ \frac{\overline{xy^2}}{\overline{y}}-2x_0\frac{\overline{xy}}{\overline{y}}+x_0^2+\frac{\overline{y^3}}{\overline{y}}-2y_0\frac{\overline{y^2}}{\overline{y}}+y_0^2+\frac{\overline{yz^2}}{\overline{y}}-2z_0\frac{\overline{yz}}{\overline{y}}+z_0^2=r^2\tag{2} $$

$$ \frac{\overline{x^2z}}{\overline{z}}-2x_0\frac{\overline{xz}}{\overline{z}}+x_0^2+\frac{\overline{y^2z}}{\overline{z}}-2y_0\frac{\overline{yz}}{\overline{z}}+y_0^2+\frac{\overline{z^3}}{\overline{x}}-2z_0\frac{\overline{yz}}{\overline{z}}+z_0^2=r^2\tag{3} $$

$$ \overline{x^2}-2x_0\overline{x}+x_0^2+\overline{y^2}-2y_0\overline{y}+y_0^2+\overline{z^2}-2z_0\overline{z}+z_0^2=r^2\tag{4} $$

分别减去 (4) 式最终可以得到如下线性方程组:

$$ \begin{bmatrix} \overline{x^2}-\bar{x}^2 & \overline{xy}-\bar{x}\cdot \bar{y} & \overline{xz}-\bar{x}*\bar{z} \\ \overline{xy}-\bar{x}*\bar{y} & \overline{y^2}-\bar{y}^2 & \overline{yz}-\bar{y}*\bar{z} \\ \overline{xz}-\bar{x}*\bar{z} & \overline{yz}-\bar{y}*\bar{z} & \overline{z^2}-\bar{z}^2 \\ \end{bmatrix} \begin{bmatrix} x_0\\ y_0\\ z_0\\ \end{bmatrix}=\frac{1}{2} \begin{bmatrix} (\overline{x^3}-\bar{x}*\overline{x^2})+(\overline{x y^2}-\bar{x}*\overline{y^2})+(\overline{xz^2}-\bar{x}*\overline{z^2}) \\ (\overline{x^2y}-\overline{x^2}*\bar{y})+(\overline{y^3}-\bar{y}*\overline{y^2})+(\overline{yz^2}-\bar{y}*\overline{z^2}) \\ (\overline{x^2z}-\overline{x^2}*\bar{z})+(\overline{zy^2}-\bar{z}*\overline{y^2})+(\overline{z^3}-\bar{z} * \overline{z^2})\\ \end{bmatrix} $$

解此线性方程组即可得到坐标 $(x_0, y_0, z_0)$,再代回 (4) 式求出 $r$ 即可。

利用法方程求解矩阵

也可以直接对球面的方程进行拟合,每一个点云坐标对应一个方程,方程数显然大于未知数,可以利用最小二乘法进行求解出一个最小值。

将方程稍微化简可得:

$$ 2ax+2by+2cz+r^2-(a^2+b^2+c^2)=x^2+y^2+z^2 $$

将每一个点坐标代入得到一个 $n\times4$ 的矩阵 A:

$$ \begin{bmatrix} x_1&y_1&z_1&1\\ x_2&y_2&z_2&1\\ \vdots&\vdots&\vdots&\vdots\\ x_n&y_n&z_n&1\\ \end{bmatrix}\begin{bmatrix}2a\\2b\\2c\\r^2-(a^2+b^2+c^2)\end{bmatrix}= \begin{bmatrix}x_1^2+y_1^2+z_1^2\\\vdots\\x_n^2+y_n^2+z_n^2\end{bmatrix} $$

方程两边同时左乘以 $A^T$ 可以得到和第一种方法一样的方程,或者直接用公式 $x=(A^TA)^{-1}A^Tb$ 进行求解,这两种方法在本质上是完全一样的。

python实现

针对第一种方法的具体代码

import numpy as np
points = np.array(coor) # coor为点云坐标的列表
points = points.astype(np.float64)  # 防止溢出
num_points = points.shape[0]
print(num_points)
x = points[:, 0]
y = points[:, 1]
z = points[:, 2]
x_avr = sum(x) / num_points
y_avr = sum(y) / num_points
z_avr = sum(z) / num_points
xx_avr = sum(x * x) / num_points
yy_avr = sum(y * y) / num_points
zz_avr = sum(z * z) / num_points
xy_avr = sum(x * y) / num_points
xz_avr = sum(x * z) / num_points
yz_avr = sum(y * z) / num_points
xxx_avr = sum(x * x * x) / num_points
xxy_avr = sum(x * x * y) / num_points
xxz_avr = sum(x * x * z) / num_points
xyy_avr = sum(x * y * y) / num_points
xzz_avr = sum(x * z * z) / num_points
yyy_avr = sum(y * y * y) / num_points
yyz_avr = sum(y * y * z) / num_points
yzz_avr = sum(y * z * z) / num_points
zzz_avr = sum(z * z * z) / num_points

A = np.array([[xx_avr - x_avr * x_avr, xy_avr - x_avr * y_avr, xz_avr - x_avr * z_avr],
              [xy_avr - x_avr * y_avr, yy_avr - y_avr * y_avr, yz_avr - y_avr * z_avr],
              [xz_avr - x_avr * z_avr, yz_avr - y_avr * z_avr, zz_avr - z_avr * z_avr]])
b = np.array([xxx_avr - x_avr * xx_avr + xyy_avr - x_avr * yy_avr + xzz_avr - x_avr * zz_avr,
              xxy_avr - y_avr * xx_avr + yyy_avr - y_avr * yy_avr + yzz_avr - y_avr * zz_avr,
              xxz_avr - z_avr * xx_avr + yyz_avr - z_avr * yy_avr + zzz_avr - z_avr * zz_avr])
# print(A, b)
b = b / 2
center = np.linalg.solve(A, b)
x0 = center[0]
y0 = center[1]
z0 = center[2]
r2 = xx_avr - 2 * x0 * x_avr + x0 * x0 + yy_avr - 2 * y0 * y_avr + y0 * y0 + zz_avr - 2 * z0 * z_avr + z0 * z0
r = r2 ** 0.5
print(center, r)

参考

https://blog.csdn.net/sunshine_zoe/article/details/78852978
https://blog.csdn.net/liyuanbhu/article/details/80201371
https://blog.csdn.net/woniu199166/article/details/79459807?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromBaidu-1&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromBaidu-1

最后修改:2021 年 11 月 19 日
你的赞赏是我前进的动力