编程语言
首页 > 编程语言> > python optimize.leastsq:将一个圆拟合到三组点

python optimize.leastsq:将一个圆拟合到三组点

作者:互联网

我正在尝试将circle fitting code用于3D数据集.我已经修改了3D点,只需在必要时添加z坐标.我的修改对一组点有效,对另一组有效.如果代码有错误,请查看代码.

import trig_items
import numpy as np
from trig_items import *
from numpy import *
from matplotlib import pyplot as p
from scipy import optimize

# Coordinates of the 3D points
##x = r_[36, 36, 19, 18, 33, 26]
##y = r_[14, 10, 28, 31, 18, 26]
##z = r_[0, 1, 2, 3, 4, 5]

x = r_[ 2144.18908574,  2144.26880854,  2144.05552972,  2143.90303742,  2143.62520676,
  2143.43628579,  2143.14005775,  2142.79919654,  2142.51436023,  2142.11240866,
  2141.68564346,  2141.29333828,  2140.92596405,  2140.3475612,   2139.90848046,
  2139.24661021,  2138.67384709,  2138.03313547,  2137.40301734,  2137.40908256,
  2137.06611224,  2136.50943781,  2136.0553113,   2135.50313189,  2135.07049922,
  2134.62098139,  2134.10459535,  2133.50838433,  2130.6600465,   2130.03537342,
  2130.04047644,  2128.83522468,  2127.79827542,  2126.43513385,  2125.36700593,
  2124.00350543,  2122.68564431,  2121.20709478,  2119.79047011,  2118.38417647,
  2116.90063343,  2115.52685778,  2113.82246629,  2112.21159431,  2110.63180117,
  2109.00713198,  2108.94434529,  2106.82777156,  2100.62343757,  2098.5090226,
  2096.28787738,  2093.91550703,  2091.66075061,  2089.15316429,  2086.69753869,
  2084.3002414,   2081.87590579,  2079.19141866,  2076.5394574,   2073.89128676,
  2071.18786213]
y = r_[ 725.74913818,  724.43874065,  723.15226506,  720.45950581,  717.77827954,
  715.07048092,  712.39633862,  709.73267688,  707.06039438,  704.43405908,
  701.80074596,  699.15371526,  696.5309022,   693.96109921,  691.35585501,
  688.83496327,  686.32148661,  683.80286662,  681.30705568,  681.30530975,
  679.66483676,  678.01922321,  676.32721779,  674.6667554,   672.9658024,
  671.23686095,  669.52021535,  667.84999077,  659.19757984,  657.46179949,
  657.45700508,  654.46901086,  651.38177517,  648.41739432,  645.32356976,
  642.39034578,  639.42628453,  636.51107198,  633.57732055,  630.63825133,
  627.75308356,  624.80162215,  622.01980232,  619.18814892,  616.37688894,
  613.57400131,  613.61535723,  610.4724493,   600.98277781,  597.84782844,
  594.75983001,  591.77946964,  588.74874068,  585.84525834,  582.92311166,
  579.99564481,  577.06666417,  574.30782762,  571.54115037,  568.79760614,
  566.08551098]
z = r_[ 339.77146775,  339.60021095,  339.47645894,  339.47130963,  339.37216218,
  339.4126132,   339.67942046,  339.40917728,  339.39500353,  339.15041461,
  339.38959195,  339.3358209,   339.47764895,  339.17854867,  339.14624071,
  339.16403926,  339.02308811,  339.27011082,  338.97684183,  338.95087698,
  338.97321177,  339.02175448,  339.02543922,  338.88725411,  339.06942374,
  339.0557553,   339.04414618,  338.89234303,  338.95572249,  339.00880416,
  339.00413073,  338.91080374,  338.98214758,  339.01135789,  338.96393537,
  338.73446188,  338.62784913,  338.72443217,  338.74880562,  338.69090173,
  338.50765186,  338.49056867,  338.57353355,  338.6196255,   338.43754399,
  338.27218569,  338.10587265,  338.43880881,  338.28962141,  338.14338705,
  338.25784154,  338.49792568,  338.15572139,  338.52967693,  338.4594245,
  338.1511823,   338.03711207,  338.19144663,  338.22022045,  338.29032321,
  337.8623197 ]

# coordinates of the barycenter
xm = mean(x)
ym = mean(y)
zm = mean(z)

### Basic usage of optimize.leastsq

def calc_R(xc, yc, zc):
    """ calculate the distance of each 3D points from the center (xc, yc, zc) """
    return sqrt((x - xc) ** 2 + (y - yc) ** 2 + (z - zc) ** 2)

def func(c):
    """ calculate the algebraic distance between the 3D points and the mean circle centered at c=(xc, yc, zc) """
    Ri = calc_R(*c)
    return Ri - Ri.mean()

center_estimate = xm, ym, zm
center, ier = optimize.leastsq(func, center_estimate)
##print center

xc, yc, zc = center
Ri       = calc_R(xc, yc, zc)
R        = Ri.mean()
residu   = sum((Ri - R)**2)
print 'R =', R

因此,对于第一组x,y,z(在代码中注释),它运行良好:输出为R = 39.0097846735.如果我使用第二组点(未注释)运行代码,则生成的半径为R = 108576.859834,这几乎是直线.我绘制了最后一个.

蓝点是给定的数据集,红色点是得到的半径R = 108576.859834的弧.很明显,给定的数据集的半径比结果小得多.

这是另一组要点.

很明显,最小二乘法不能正常工作.

请帮我解决这个问题.

UPDATE

这是我的解决方案:

### fit 3D arc into a set of 3D points             ###
### output is the centre and the radius of the arc ###
def fitArc3d(arr, eps = 0.0001):
    # Coordinates of the 3D points
    x = numpy.array([arr[k][0] for k in range(len(arr))])
    y = numpy.array([arr[k][4] for k in range(len(arr))])
    z = numpy.array([arr[k][5] for k in range(len(arr))])
    # coordinates of the barycenter
    xm = mean(x)
    ym = mean(y)
    zm = mean(z)
    ### gradient descent minimisation method ###
    pnts = [[x[k], y[k], z[k]] for k in range(len(x))]
    meanP = Point(xm, ym, zm) # mean point
    Ri = [Point(*meanP).distance(Point(*pnts[k])) for k in range(len(pnts))] # radii to the points
    Rm = math.fsum(Ri) / len(Ri) # mean radius
    dR = Rm + 10 # difference between mean radii
    alpha = 0.1
    c = meanP
    cArr = []
    while dR  > eps:
        cArr.append(c)
        Jx = math.fsum([2 * (x[k] - c[0]) * (Ri[k] - Rm) / Ri[k] for k in range(len(Ri))])
        Jy = math.fsum([2 * (y[k] - c[1]) * (Ri[k] - Rm) / Ri[k] for k in range(len(Ri))])
        Jz = math.fsum([2 * (z[k] - c[2]) * (Ri[k] - Rm) / Ri[k] for k in range(len(Ri))])
        gradJ = [Jx, Jy, Jz] # find gradient
        c = [c[k] + alpha * gradJ[k] for k in range(len(c)) if len(c) == len(gradJ)] # find new centre point
        Ri = [Point(*c).distance(Point(*pnts[k])) for k in range(len(pnts))] # calculate new radii
        RmOld = Rm
        Rm = math.fsum(Ri) / len(Ri) # calculate new mean radius
        dR = abs(Rm - RmOld) # new difference between mean radii

    return Point(*c), Rm

它不是非常优化的代码(我没有时间对它进行微调)但它有效.

解决方法:

我猜问题是数据和相应的算法.如果最小二乘法产生局部抛物线最小值,则最小二乘法可以正常工作,这样简单的梯度法大致方向最小.不幸的是,您的数据并不一定如此.你可以通过保持对xc和yc的一些粗略估计来固定并绘制平方残差的总和作为zc和R的函数来检查这一点.我得到一个回旋镖形状的最小值.根据您的起始参数,您可能会在一个远离实际最小值的分支中结束.一旦进入山谷,这可能非常平坦,因此您超过最大迭代次数或获得在算法容差范围内接受的内容.一如既往,思考越好,你的起始参数越好.不幸的是,你只有一个圆弧,所以很难变得更好.我不是Python的专家,但我认为leastsq允许你使用Jacobian和Gradient方法.尽量使用公差.
简而言之:代码看起来对我来说基本上很好,但是你的数据是病态的,你必须使代码适应那种数据.
来自Karimäki的2D中有一个非迭代解决方案,也许你可以适应
this method到3D.你也可以看看this.当然你会找到更多的文献.

我刚刚使用Simplex-Algorithm检查了数据.正如我所说,最低限度是表现不佳.在这里看到一些剩余功能的削减.只有在xy平面上才能得到一些合理的行为. zr-和xr-平面的特性使得发现过程非常困难.

因此,在一开始,单纯形算法找到了几个几乎稳定的解.您可以在下图中看到它们是平坦的步骤(蓝色x,紫色y,黄色z,绿色R).最后,算法必须沿着几乎平坦但非常伸展的山谷向下走,导致z和R的最终转换.然而,如果公差不足,我希望许多区域看起来像一个解决方案.在标准公差为10 ^ -5的情况下,算法在大约350次迭代后停止.我必须将它设置为10 ^ -10才能得到这个解决方案,即[1899.32,741.874,298.696,248.956],这似乎相当不错.

更新

如前所述,解决方案取决于工作精度和要求的准确性.所以你的手工制作渐变方法可能会更好,因为这些值与内置最小二乘拟合相比是不同的.不过,这是我的版本两步合适.首先,我将飞机安装到数据上.在下一步中,我在这个平面内安装一个圆.两个步骤都使用最小二乘法.这一次它起作用,因为每一步都避免了形状严重的极小. (当然,如果弧段变小并且数据实际上位于一条直线上,则平面拟合会遇到问题.但是对于所有算法都会发生这种情况)

from math import *
from matplotlib import pyplot as plt
from scipy import optimize
import numpy as np
from mpl_toolkits.mplot3d import Axes3D
import pprint as pp

dataTupel=zip(xs,ys,zs) #your data from above

# Fitting a plane first
# let the affine plane be defined by two vectors, 
# the zero point P0 and the plane normal n0
# a point p is member of the plane if (p-p0).n0 = 0 

def distanceToPlane(p0,n0,p):
    return np.dot(np.array(n0),np.array(p)-np.array(p0))    

def residualsPlane(parameters,dataPoint):
    px,py,pz,theta,phi = parameters
    nx,ny,nz =sin(theta)*cos(phi),sin(theta)*sin(phi),cos(theta)
    distances = [distanceToPlane([px,py,pz],[nx,ny,nz],[x,y,z]) for x,y,z in dataPoint]
    return distances

estimate = [1900, 700, 335,0,0] # px,py,pz and zeta, phi
#you may automize this by using the center of mass data
# note that the normal vector is given in polar coordinates
bestFitValues, ier = optimize.leastsq(residualsPlane, estimate, args=(dataTupel))
xF,yF,zF,tF,pF = bestFitValues

point  = [xF,yF,zF]
normal = [sin(tF)*cos(pF),sin(tF)*sin(pF),cos(tF)]

# Fitting a circle inside the plane
#creating two inplane vectors
sArr=np.cross(np.array([1,0,0]),np.array(normal))#assuming that normal not parallel x!
sArr=sArr/np.linalg.norm(sArr)
rArr=np.cross(sArr,np.array(normal))
rArr=rArr/np.linalg.norm(rArr)#should be normalized already, but anyhow


def residualsCircle(parameters,dataPoint):
    r,s,Ri = parameters
    planePointArr = s*sArr + r*rArr + np.array(point)
    distance = [ np.linalg.norm( planePointArr-np.array([x,y,z])) for x,y,z in dataPoint]
    res = [(Ri-dist) for dist in distance]
    return res

estimateCircle = [0, 0, 335] # px,py,pz and zeta, phi
bestCircleFitValues, ier = optimize.leastsq(residualsCircle, estimateCircle,args=(dataTupel))

rF,sF,RiF = bestCircleFitValues
print bestCircleFitValues

# Synthetic Data
centerPointArr=sF*sArr + rF*rArr + np.array(point)
synthetic=[list(centerPointArr+ RiF*cos(phi)*rArr+RiF*sin(phi)*sArr) for phi in np.linspace(0, 2*pi,50)]
[cxTupel,cyTupel,czTupel]=[ x for x in zip(*synthetic)]

### Plotting
d = -np.dot(np.array(point),np.array(normal))# dot product
# create x,y mesh
xx, yy = np.meshgrid(np.linspace(2000,2200,10), np.linspace(540,740,10))
# calculate corresponding z
# Note: does not work if normal vector is without z-component
z = (-normal[0]*xx - normal[1]*yy - d)/normal[2]

# plot the surface, data, and synthetic circle
fig = plt.figure()
ax = fig.add_subplot(211, projection='3d')
ax.scatter(xs, ys, zs, c='b', marker='o')
ax.plot_wireframe(xx,yy,z)
ax.set_xlabel('X Label')
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')
bx = fig.add_subplot(212, projection='3d')
bx.scatter(xs, ys, zs, c='b', marker='o')
bx.scatter(cxTupel,cyTupel,czTupel, c='r', marker='o')
bx.set_xlabel('X Label')
bx.set_ylabel('Y Label')
bx.set_zlabel('Z Label')
plt.show()

它的半径为245.这与其他方法的结果相近(249).因此,在误差范围内,我得到相同的结果.

绘制的结果看起来合理.
希望这可以帮助.

标签:python,optimization,curve-fitting,least-squares
来源: https://codeday.me/bug/20190517/1122848.html