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