import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from scipy.optimize import least_squares
from scipy.linalg import svd
import matplotlib
import pandas as pd
# 设置字体为黑体（SimHei），请确保该字体在您的系统上可用
matplotlib.rcParams['font.family'] = 'SimHei'
matplotlib.rcParams['axes.unicode_minus'] = False # 显示负号
def ellipsoid_fit(points):
"""
使用最小二乘法拟合椭球
参数:
points: (N, 3)数组，包含x, y, z坐标
返回:
center: 椭球中心 [x0, y0, z0]
radii: 椭球半径 [a, b, c]
rotation: 旋转矩阵 (3, 3)
"""
# 将数据点移动到原点附近
center = np.mean(points, axis=0)
points_centered = points - center
# 构建设计矩阵
x, y, z = points_centered[:, 0], points_centered[:, 1], points_centered[:, 2]
A = np.column_stack([x ** 2, y ** 2, z ** 2, 2 * x * y, 2 * x * z, 2 * y * z, 2 * x, 2 * y, 2 * z])
# 右侧是常数1
b = np.ones(len(points))
# 使用SVD解决最小二乘问题
u, s, vh = svd(A, full_matrices=False)
params = vh.T @ (np.diag(1 / s) @ (u.T @ b))
# 提取椭球参数
A11, A22, A33, A12, A13, A23, A14, A24, A34 = params
A44 = -1
# 构建二次型矩阵
M = np.array([[A11, A12, A13, A14],
[A12, A22, A23, A24],
[A13, A23, A33, A34],
[A14, A24, A34, A44]])
# 提取中心
T = -np.linalg.inv(M[:3, :3]) @ M[:3, 3]
center_adjusted = T + center
# 构建平移后的矩阵
M2 = M.copy()
M2[:3, 3] = 0
M2[3, :3] = 0
M2[3, 3] = -1
T_mat = np.eye(4)
T_mat[:3, 3] = T
M_translated = T_mat.T @ M2 @ T_mat
# 对角化得到半径和旋转矩阵
eigenvalues, eigenvectors = np.linalg.eig(-M_translated[:3, :3] / M_translated[3, 3])
# 确保特征值为正
radii = np.sqrt(1.0 / np.abs(eigenvalues))
rotation = eigenvectors
return center_adjusted, radii, rotation
def ellipsoid_residuals(params, points):
"""
计算点到椭球面的距离残差
"""
x0, y0, z0, a, b, c = params[:6]
# 简单假设没有旋转，如果需要考虑旋转，需要更复杂的参数化
dx = points[:, 0] - x0
dy = points[:, 1] - y0
dz = points[:, 2] - z0
# 计算残差
residuals = (dx ** 2 / a ** 2 + dy ** 2 / b ** 2 + dz ** 2 / c ** 2) - 1
return residuals
def plot_ellipsoid(center, radii, rotation, ax, color='r', alpha=0.2):
"""
在3D图上绘制椭球
"""
u = np.linspace(0, 2 * np.pi, 50)
v = np.linspace(0, np.pi, 50)
x = radii[0] * np.outer(np.cos(u), np.sin(v))
y = radii[1] * np.outer(np.sin(u), np.sin(v))
z = radii[2] * np.outer(np.ones_like(u), np.cos(v))
# 旋转和平移
for i in range(len(x)):
for j in range(len(x)):
[x[i, j], y[i, j], z[i, j]] = rotation @ [x[i, j], y[i, j], z[i, j]] + center
ax.plot_surface(x, y, z, color=color, alpha=alpha)
# 主程序
if __name__ == "__main__":
# 这里替换为您的实际数据
# 数据格式应该是N行3列的数组，每行是一个点的x,y,z坐标
# 例如:
# your_data = np.array([[x1, y1, z1], [x2, y2, z2], ...])
# 如果没有真实数据，我们可以生成一些示例数据
'''
np.random.seed(42)
n_points = 200
true_center = [1, 2, 3]
true_radii = [4, 3, 2]
# 生成椭球上的点并添加噪声
u = np.random.rand(n_points) * 2 * np.pi
v = np.random.rand(n_points) * np.pi
x = true_radii[0] * np.cos(u) * np.sin(v) + true_center[0] + np.random.normal(0, 0.1, n_points)
y = true_radii[1] * np.sin(u) * np.sin(v) + true_center[1] + np.random.normal(0, 0.1, n_points)
z = true_radii[2] * np.cos(v) + true_center[2] + np.random.normal(0, 0.1, n_points)
your_data = np.column_stack([x, y, z])
'''
your_data = np.array([
[-42.653, -23.067, 3.040],
[-40.160, -22.920, -17.653],
[-27.427, -22.320, -35.760],
[-7.373, -21.520, -44.493],
[13.800, -20.773, -42.227],
[31.427, -20.067, -28.813],
[40.453, -19.747, -8.360],
[38.813, -19.867, 12.933],
[25.760, -20.387, 32.160],
[6.307, -21.280, 41.333],
[-13.653, -22.027, 39.747],
[-31.853, -22.560, 27.493],
[-42.440, -22.947, 5.173],
[-42.253, -16.160, 17.427],
[-46.120, -16.360, -5.973],
[-37.320, -15.867, -30.333],
[-21.373, -14.933, -44.080],
[3.307, -14.093, -48.813],
[25.773, -13.320, -39.587],
[40.147, -12.653, -21.893],
[44.520, -12.560, 1.920],
[34.933, -12.987, 27.893],
[16.853, -13.773, 41.773],
[-3.973, -14.387, 45.333],
[-27.507, -14.893, 36.480],
[-41.813, -15.253, 18.907],
[-42.453, -4.053, 23.413],
[-49.187, -3.947, 1.133],
[-44.373, -3.720, -24.573],
[-26.933, -3.160, -44.680],
[-3.893, -2.227, -52.360],
[22.813, -0.907, -46.053],
[40.573, -0.040, -28.560],
[48.000, 0.280, -4.453],
[43.200, 0.227, 20.293],
[25.293, -0.813, 40.773],
[1.387, -2.000, 48.200],
[-25.027, -3.173, 41.307],
[-42.267, -3.933, 23.747],
[-42.680, 10.373, 22.053],
[-49.000, 10.160, -1.693],
[-43.507, 10.413, -26.373],
[-25.040, 11.333, -46.187],
[0.173, 12.200, -52.787],
[24.907, 13.320, -45.133],
[41.240, 14.013, -27.480],
[47.627, 14.213, -4.427],
[41.200, 13.920, 22.827],
[23.733, 13.093, 40.920],
[-1.733, 12.013, 47.627],
[-23.507, 11.013, 41.427],
[-41.707, 10.213, 23.600],
[-42.413, 22.000, 13.893],
[-44.973, 22.027, -10.213],
[-37.573, 22.373, -29.733],
[-18.440, 23.253, -45.947],
[5.227, 24.253, -49.080],
[25.853, 25.040, -40.453],
[41.400, 25.613, -19.613],
[43.920, 25.653, 3.973],
[35.973, 25.280, 24.733],
[14.947, 24.093, 41.360],
[-7.427, 23.053, 43.093],
[-27.427, 22.493, 33.840],
[-42.453, 22.000, 13.053],
[-41.747, 28.627, 0.373],
[-38.853, 28.893, -19.027],
[-24.240, 29.573, -38.267],
[-5.960, 30.293, -45.560],
[16.413, 31.120, -42.000],
[33.027, 31.413, -28.427],
[41.107, 31.707, -6.213],
[38.440, 31.667, 13.560],
[22.267, 30.987, 33.547],
[3.440, 30.200, 39.973],
[-12.773, 29.613, 37.973],
[-32.133, 29.173, 24.267],
[-41.400, 28.880, 1.080],
[-40.533, 29.053, -13.480],
[-31.000, 29.680, -31.933],
[-12.547, 30.427, -44.160],
[5.440, 31.160, -45.413],
[26.293, 31.747, -35.547],
[39.080, 31.960, -15.227],
[40.827, 32.080, 3.787],
[31.707, 31.573, 25.440],
[12.573, 30.773, 38.480],
[-8.867, 29.800, 39.320],
[-26.640, 29.480, 30.520],
[-39.000, 29.120, 13.147],
[-41.053, 29.013, -10.067],
[-41.507, 21.013, -23.080],
[-25.813, 21.600, -42.040],
[-3.693, 22.520, -49.747],
[20.347, 23.507, -44.587],
[37.933, 24.107, -27.827],
[44.947, 24.067, -2.333],
[39.147, 23.680, 21.267],
[27.453, 23.253, 35.187],
[3.573, 22.253, 44.733],
[-21.600, 21.053, 39.107],
[-38.360, 20.933, 23.587],
[-45.920, 20.693, 2.027],
[-41.613, 20.787, -23.227],
[-40.707, 9.080, -31.200],
[-22.067, 9.973, -47.747],
[3.707, 11.000, -52.507],
[25.587, 11.653, -44.813],
[44.320, 12.067, -21.573],
[47.760, 11.773, 2.493],
[37.680, 11.307, 29.520],
[18.147, 10.453, 44.653],
[-7.440, 9.427, 47.613],
[-28.853, 8.533, 38.640],
[-43.333, 8.173, 22.173],
[-49.373, 8.187, -5.533],
[-41.867, 8.333, -29.453],
[-41.067, -5.000, -29.973],
[-22.947, -4.347, -46.920],
[1.147, -3.427, -52.253],
[27.840, -2.507, -42.440],
[42.613, -2.280, -23.693],
[47.360, -2.347, 0.400],
[38.227, -2.933, 28.133],
[19.067, -3.613, 44.053],
[-1.720, -4.493, 48.280],
[-27.560, -5.347, 39.867],
[-43.960, -5.840, 20.933],
[-49.333, -5.640, -6.400],
[-41.040, -5.440, -30.147],
[-41.693, -17.280, -21.493],
[-27.667, -16.707, -39.520],
[-6.013, -15.853, -48.040],
[17.493, -14.920, -44.040],
[35.507, -14.280, -28.653],
[43.667, -14.160, -4.160],
[39.240, -14.427, 19.413],
[25.320, -15.133, 36.280],
[4.733, -16.013, 44.653],
[-19.773, -17.053, 40.227],
[-37.373, -17.453, 25.013],
[-45.480, -17.707, 3.080],
[-41.680, -17.493, -21.627],
[-41.680, -23.707, -10.640],
[-33.520, -23.307, -28.867],
[-18.493, -22.667, -41.027],
[3.933, -21.827, -44.400],
[24.907, -21.200, -34.893],
[37.693, -20.640, -15.827],
[39.467, -20.840, 8.173],
[31.373, -21.173, 25.827],
[13.453, -21.840, 39.173],
[-8.587, -22.693, 40.827],
[-26.600, -23.080, 32.507],
[-40.667, -23.493, 12.400],
[-41.787, -23.707, -10.787],
[-42.200, -23.413, 5.160],
[-40.213, -23.320, -16.773],
[-28.427, -22.827, -34.387],
[-11.240, -22.093, -43.707],
[13.987, -21.227, -41.667],
[31.707, -20.613, -27.867],
[40.253, -20.307, -7.840],
[39.453, -20.373, 10.000],
[25.453, -20.893, 32.440],
[4.467, -21.733, 41.387],
[-18.533, -22.373, 37.573],
[-34.027, -22.853, 24.707],
[-42.587, -23.120, 2.907]
])
# 拟合椭球
center, radii, rotation = ellipsoid_fit(your_data)
print(f"拟合中心: {center}")
print(f"拟合半径: {radii}")
print(f"旋转矩阵:\n{rotation}")
# 计算残差
residuals = ellipsoid_residuals(np.concatenate([center, radii]), your_data)
rmse = np.sqrt(np.mean(residuals ** 2))
print(f"RMSE: {rmse}")
# 绘制结果
fig = plt.figure(figsize=(15, 5))
# 原始数据
ax1 = fig.add_subplot(131, projection='3d')
ax1.scatter(your_data[:, 0], your_data[:, 1], your_data[:, 2], c='b', marker='o', alpha=0.6)
ax1.set_title('原始数据')
ax1.set_xlabel('X')
ax1.set_ylabel('Y')
ax1.set_zlabel('Z')
# 拟合后的椭球
ax2 = fig.add_subplot(132, projection='3d')
ax2.scatter(your_data[:, 0], your_data[:, 1], your_data[:, 2], c='b', marker='o', alpha=0.6)
plot_ellipsoid(center, radii, rotation, ax2, color='r', alpha=0.2)
ax2.set_title('拟合椭球')
ax2.set_xlabel('X')
ax2.set_ylabel('Y')
ax2.set_zlabel('Z')
# 残差图
ax3 = fig.add_subplot(133)
ax3.plot(residuals, 'o-')
ax3.axhline(y=0, color='r', linestyle='--')
ax3.set_title('残差图')
ax3.set_xlabel('数据点索引')
ax3.set_ylabel('残差')
ax3.grid(True)
plt.tight_layout()
plt.savefig('ellipsoid_fit_result.png', dpi=300)
plt.show()
# 输出残差统计信息
print("\n残差统计:")
print(f"最大正残差: {np.max(residuals)}")
print(f"最大负残差: {np.min(residuals)}")
print(f"平均残差: {np.mean(residuals)}")
print(f"残差标准差: {np.std(residuals)}") 原始数据哪怕是拟合成椭球，能不能也画一个图像，想看看差距
扫一扫
6808
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
