def fitRigidSize(data, target, x0=None, xtol=1e-3, maxfev=0, verbose=0):
""" fits for tx,ty,tz,rx,ry,rz,s to transform points in data to points
in target. Points in data and target are assumed to correspond by
order
"""
if x0 == None:
x0 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
def obj(x):
dataT = transformRigidSize3D(data, x)
d = ((dataT - target) ** 2.0).sum(1)
return d
x0 = scipy.array(x0)
if verbose:
rms0 = scipy.sqrt(obj(x0).mean())
print "initial RMS:", rms0
xOpt = leastsq(obj, x0, xtol=xtol, maxfev=maxfev)[0]
if verbose:
rmsOpt = scipy.sqrt(obj(xOpt).mean())
print "final RMS:", rmsOpt
dataFitted = transformRigidSize3D(data, xOpt)
return xOpt, dataFitted
def obj(x):
dataT = transformRigidSize3D(data, x)
d = ((dataT - target) ** 2.0).sum(1)
return d
