python中使用scipy.optimize.leastsq进行球面、圆柱面、平面拟合

scipy.optimize.leastsq官方文档
之前自己用最小二乘进行过球面、柱面和平面拟合，球面和平面效果都很好，但是柱面效果一直不好（对初值十分敏感，迭代过程中经常出现奇异矩阵，也有可能是自己写的算法没有考虑数值误差）。所以采用scipy自带的最小二乘拟合进行曲面拟合，当对初值进行一定限制时，拟合效果很好。

球面拟合

球面方程：
(x−x0)2+(y−y0)2+(z−z0)2=r02(x-x_0)^2+(y-y_0)^2+(z-z_0)^2=r_0^2(x−x0)2+(y−y0)2+(z−z0)2=r02
误差函数：
F(x0,y0,z0,r0)=(x−x0)2+(y−y0)2+(z−z0)2−r02F(x_0,y_0,z_0,r_0)=(x-x_0)^2+(y-y_0)^2+(z-z_0)^2-r_0^2F(x0,y0,z0,r0)=(x−x0)2+(y−y0)2+(z−z0)2−r02
前面为使用leastsq拟合，先定义误差函数，再通过leastsq进行拟合；后面为自己写的代码，直接计算不用赋初值，具体算法参考球面数据拟合算法简介

############   leastsq拟合  ###############
def spherrors(para, points):"""球面拟合误差"""x0, y0, z0, r0 = parareturn (points[:, 0] - x0) ** 2 + (points[:, 1] - y0) ** 2 + (points[:, 2] - z0) ** 2 - r0 ** 2
# 计算点集的x,y,z均值，用于拟合参数初值
xm = np.mean(points[:,0])    # 特征点集x均值
ym = np.mean(points[:,1])
zm = np.mean(points[:,2])
# 最小二乘拟合
tparas = optimize.leastsq(spherrors, [xm, ym, zm, r0], points, full_output=1)
paras = tparas[0]
# 计算球度误差
es = np.mean(np.abs(tparas[2]['fvec'])) / paras[3]    # 'fvec'即为spherrors的值###############    自己写的    ############
def sphere_surface(points):"""球面拟合"""x = points[:, 0]y = points[:, 1]z = points[:, 2]# 计算拟合矩阵A，数组b的参数xm = np.mean(x)ym = np.mean(y)zm = np.mean(z)xym = np.mean(x * y)xzm = np.mean(x * z)yzm = np.mean(y * z)x2m = np.mean(x * x)y2m = np.mean(y * y)z2m = np.mean(z * z)x2ym = np.mean(x * x * y)x2zm = np.mean(x * x * z)y2xm = np.mean(y * y * x)y2zm = np.mean(y * y * z)z2xm = np.mean(z * z * x)z2ym = np.mean(z * z * y)x3m = np.mean(x * x * x)y3m = np.mean(y * y * y)z3m = np.mean(z * z * z)A = np.array([[x2m - xm * xm, xym - xm * ym, xzm - xm * zm], [xym - xm * ym, y2m - ym * ym, yzm - ym * zm],[xzm - xm * zm, yzm - ym * zm, z2m - zm * zm]])b = 0.5 * np.array([x3m - xm * x2m + y2xm - xm * y2m + z2xm - xm * z2m, x2ym - x2m * ym + y3m - ym * y2m + z2ym - ym * z2m,x2zm - x2m * zm + y2zm - y2m * zm + z3m - z2m * zm])# 求解球心s = linalg.solve(A, b)# 求解半径R = np.sqrt(x2m - 2 * s[0] * xm + s[0] ** 2 + y2m - 2 * s[1] * ym + s[1] ** 2 + z2m - 2 * s[2] * zm + s[2] ** 2)return s, R

圆柱拟合

圆柱面方程：
(x−x0)2+(y−y0)2+(z−z0)2−r02=[l(x−x0)+m(y−y0)+n(z−z0)]2(x-x_0)^2+(y-y_0)^2+(z-z_0)^2-r_0^2=[l(x-x_0)+m(y-y_0)+n(z-z_0)]^2(x−x0)2+(y−y0)2+(z−z0)2−r02=[l(x−x0)+m(y−y0)+n(z−z0)]2
误差函数：
F(x0,y0,z0,l,m,n,r0)=(x−x0)2+(y−y0)2+(z−z0)2−r02−[l(x−x0)+m(y−y0)+n(z−z0)]2F(x_0,y_0,z_0,l,m,n,r_0)=(x-x_0)^2+(y-y_0)^2+(z-z_0)^2-r_0^2-[l(x-x_0)+m(y-y_0)+n(z-z_0)]^2F(x0,y0,z0,l,m,n,r0)=(x−x0)2+(y−y0)2+(z−z0)2−r02−[l(x−x0)+m(y−y0)+n(z−z0)]2
圆柱面拟合对初值十分敏感，需要根据已知信息对初值进行限定，此处操作为：根据圆柱面上点公法线（即与所有点法向量点乘和最小的向量）中主分量（最大分量）来减少参数（如公法线中x分量最大，则轴线一定与yz平面相交，则可减少参数x0x_0x0，同时根据公法线主分量方向估计圆柱的半径）

def xclinerrors(para, points): """法向量x矢量最大，圆柱面拟合误差"""# points为由点集构成的n*3矩阵，每一行为一个点的x,y,z坐标值x0 = 0y0, z0, a0, b0, c0, r0 = parareturn (points[:, 0] - x0) ** 2 + (points[:, 1] - y0) ** 2 + (points[:, 2] - z0) ** 2 - (a0 * (points[:, 0] - x0) + b0 * (points[:, 1] - y0) + c0 * (points[:, 2] - z0)) ** 2 - r0 ** 2def yclinerrors(para, points):"""法向量y矢量最大，圆柱面拟合误差"""y0 = 0x0, z0, a0, b0, c0, r0 = parareturn (points[:, 0] - x0) ** 2 + (points[:, 1] - y0) ** 2 + (points[:, 2] - z0) ** 2 - (a0 * (points[:, 0] - x0) + b0 * (points[:, 1] - y0) + c0 * (points[:, 2] - z0)) ** 2 - r0 ** 2def zclinerrors(para, points):"""法向量z矢量最大，圆柱面拟合误差"""z0 = 0x0, y0, a0, b0, c0, r0 = parareturn (points[:, 0] - x0) ** 2 + (points[:, 1] - y0) ** 2 + (points[:, 2] - z0) ** 2 - (a0 * (points[:, 0] - x0) + b0 * (points[:, 1] - y0) + c0 * (points[:, 2] - z0)) ** 2 - r0 ** 2# 计算点集的x,y,z均值，x，y的范围，用于拟合参数初值
xm = np.mean(points[:,0])    # 特征点集x均值
ym = np.mean(points[:,1])
zm = np.mean(points[:,2])
xrange = np.max(points[:,0]) - np.min(points[:,0])    # 特征点集x范围
yrange = np.max(points[:,1]) - np.min(points[:,1])# 最小二乘拟合
# x分量最大，则法向量必然与yz平面相交，轴点初值中x可设为0
# cnv为公法线
if cnv[0] >= cnv[1] and cnv[0] >= cnv[2]:   tparac = optimize.leastsq(xclinerrors, [ym, zm, cnv[0], cnv[1], cnv[2], yrange/2], points,full_output=1)parac = np.array([0, tparac[0][0], tparac[0][1], tparac[0][2], tparac[0][3], tparac[0][4], tparac[0][5]])# 圆度误差均值ec = np.mean(np.abs(tparac[2]['fvec'])) / parac[6]if cnv[1] >= cnv[0] and cnv[1] >= cnv[2]:tparac = optimize.leastsq(yclinerrors, [xm, zm, cnv[0], cnv[1], cnv[2], xrange/2], points,full_output=1)parac = np.array([tparac[0][0], 0, tparac[0][1], tparac[0][2], tparac[0][3], tparac[0][4], tparac[0][5]])ec = np.mean(np.abs(tparac[2]['fvec'])) / parac[6]if cnv[2] >= cnv[0] and cnv[2] >= cnv[1]:tparac = optimize.leastsq(zclinerrors, [xm, ym, cnv[0], cnv[1], cnv[2], (xrange + yrange) / 2],points,full_output=1)parac = np.array([tparac[0][0], tparac[0][1], 0, tparac[0][2], tparac[0][3], tparac[0][4], tparac[0][5]])ec = np.mean(np.abs(tparac[2]['fvec'])) / parac[6]

############### 自己写的，效果不好，仅供参考
最小二乘原理拟合圆柱计算方法

def cylindrical_surface(pm):"""采用最小二乘法拟合圆柱面"""# pm为由点集构成的n*3矩阵，每一行为一个点的x,y,z坐标值para = np.array([1, 1, 1, 1, 1, 1], dtype=float)m = np.zeros((pm.shape[0], 6), dtype=float)B = np.zeros((pm.shape[0],), dtype=float)for i in range(5):for i in range(pm.shape[0]):x = pm[i, 0]y = pm[i, 1]z = pm[i, 2]x0 = para[0]y0 = para[1]a0 = para[2]b0 = para[3]c0 = para[4]r0 = para[5]z0 = 0f0 = pow(x - x0, 2) + pow(y - y0, 2) + pow(z - z0, 2) - pow((a0 * (x - x0) + b0 * (y - y0) + c0 * (z - z0)), 2) - r0 * r0B[i] = -f0m[i, 0] = (a0 * (a0 * (x - x0) + b0 * (y - y0) + c0 * (z - z0)) - (x - x0)) * 2m[i, 1] = (b0 * (a0 * (x - x0) + b0 * (y - y0) + c0 * (z - z0)) - (y - y0)) * 2m[i, 2] = -(a0 * (x - x0) + b0 * (y - y0) + c0 * (z - z0)) * (x - x0) * 2m[i, 3] = -(a0 * (x - x0) + b0 * (y - y0) + c0 * (z - z0)) * (y - y0) * 2m[i, 4] = -(a0 * (x - x0) + b0 * (y - y0) + c0 * (z - z0)) * (z - z0) * 2m[i, 5] = -2 * r0mt = m.transpose()para = np.dot(np.dot(np.linalg.inv(np.dot(mt, m)), mt), B)return para

##平面拟合
平面方程：
z=a0x+a1y+a2z=a_0x+a_1y+a_2z=a0x+a1y+a2
误差函数：
F(a0,a1,a2)=a0x+a1y+a2−zF(a_0,a_1,a_2)=a_0x+a_1y+a_2-zF(a0,a1,a2)=a0x+a1y+a2−z

def planeerrors(para, points):"""平面误差"""a0, a1, a2 = parareturn a0 * points[:, 0] + a1 * points[:, 1] + a2 - points[:, 2]tparap = optimize.leastsq(planeerrors, [1, 1, 1], points)
para = tparap[0]