这更多的是一个手工制作的使用基本优化的解决方案。这是直截了当的。只需测量一点到要拟合的线的距离,并使用基本方法最小化加权距离optimize.leastsq
。代码如下:
# -*- coding: utf-8 -*
from matplotlib import pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.cm as cm
from scipy import optimize
import numpy as np
def rnd( a ):
return a * ( 1 - 2 * np.random.random() )
def affine_line( s, theta, phi, x0, y0, z0 ):
a = np.sin( theta) * np.cos( phi )
b = np.sin( theta) * np.sin( phi )
c = np.cos( theta )
return np.array( [ s * a + x0, s * b + y0, s * c + z0 ] )
def point_to_line_distance( x , y, z , theta, phi, x0, y0, z0 ):
xx = x - x0
yy = y - y0
zz = z - z0
a = np.sin( theta) * np.cos( phi )
b = np.sin( theta) * np.sin( phi )
c = np.cos( theta )
r = np.array( [ xx, yy, zz ] )
t = np.array( [ a, b, c ] )
return np.linalg.norm( r - np.dot( r, t) * t )
def residuals( parameters, fixpoint, data, weights=None ):
theta, phi = parameters
x0, y0, z0 = fixpoint
if weights is None:
w = np.ones( len( data ) )
else:
w = np.array( weights )
diff = np.array( [ point_to_line_distance( x , y, z , theta, phi , *fixpoint ) for x, y, z in data ] )
diff = diff * w
return diff
### some test data
fixpoint = [ 1, 2 , -.3 ]
trueline = np.array( [ affine_line( s, .7, 1.7, *fixpoint ) for s in np.linspace( -1, 2, 50 ) ] )
rndData = np.array( [ np.array( [ a + rnd( .6), b + rnd( .35 ), c + rnd( .45 ) ] ) for a, b, c in trueline ] )
zData = [ 20 * point_to_line_distance( x , y, z , .7, 1.7, *fixpoint ) for x, y, z in rndData ]
### unweighted
bestFitValuesUW, ier= optimize.leastsq(residuals, [ 0, 0],args=( fixpoint, rndData ) )
print bestFitValuesUW
uwLine = np.array( [ affine_line( s, bestFitValuesUW[0], bestFitValuesUW[1], *fixpoint ) for s in np.linspace( -2, 2, 50 ) ] )
### weighted ( chose inverse distance as weight....would be charge in OP's case )
bestFitValuesW, ier= optimize.leastsq(residuals, [ 0, 0],args=( fixpoint, rndData, [ 1./s for s in zData ] ) )
print bestFitValuesW
wLine = np.array( [ affine_line( s, bestFitValuesW[0], bestFitValuesW[1], *fixpoint ) for s in np.linspace( -2, 2, 50 ) ] )
### plotting
fig = plt.figure()
ax = fig.add_subplot( 1, 1, 1, projection='3d' )
ax.plot( *np.transpose(trueline ) )
ax.scatter( *fixpoint, color='k' )
ax.scatter( rndData[::,0], rndData[::,1], rndData[::,2] , c=zData, cmap=cm.jet )
ax.plot( *np.transpose( uwLine ) )
ax.plot( *np.transpose( wLine ) )
ax.set_xlim( [ 0, 2.5 ] )
ax.set_ylim( [ 1, 3.5 ] )
ax.set_zlim( [ -1.25, 1.25 ] )
plt.show()
返回
>> [-0.68236386 -1.3057938 ]
>> [-0.70928735 -1.4617517 ]
![results](https://i.stack.imgur.com/Zfhkg.png)
固定点以黑色显示。原来的线是蓝色的。未加权和加权拟合分别为橙色和绿色。数据根据与线的距离进行着色。