问题

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

球面方程为:

其中 是我们要求的参数。

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

构造方程:

使方程 值最小的参数 就是我们的所求。 求偏导并使其等于0:

化简得:

整理得:

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

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

利用最小二乘法求解矩阵

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

将方程稍微化简可得:

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

方程两边同时左乘以 可以得到和第一种方法一样的方程,或者直接用公式 进行求解,这两种方法在本质上是完全一样的。

python实现

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

import numpy as np
points = np.array(coor) # coor为点云坐标的列表
points = points.astype(np.uint64)  # 防止溢出
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

Last modification:June 30th, 2020 at 03:52 pm