def residuals(params,x,y,z):
xo = params[0]
xs = params[1]
yo = params[2]
ys = params[3]
zo = params[4]
zs = params[5]
xys = params[6]
xzs = params[7]
yzs = params[8]
xc = empty(shape(x))
yc = empty(shape(y))
zc = empty(shape(z))
for i in range(len(x)):
_x = x[i] - xo
_y = y[i] - yo
_z = z[i] - zo
xc[i] = _x * (xs + _y * xys + _z * xzs)
yc[i] = _y * (ys + _z * yzs)
zc[i] = _z * (zs)
res = []
for i in range(len(xc)):
norm = l2norm(array([xc[i],yc[i],zc[i]])) - 1.0
res.append(norm)
return array(res)
def residuals(params,x,y,z):
xo = params[0]
xs = params[1]
yo = params[2]
ys = params[3]
zo = params[4]
zs = params[5]
xc = empty(shape(x))
for i in range(len(x)):
xc[i] = (x[i] - xo) * xs
yc = empty(shape(y))
for i in range(len(y)):
yc[i] = (y[i] - yo) * ys
zc = empty(shape(z))
for i in range(len(z)):
zc[i] = (z[i] - zo) * zs
res = []
for i in range(len(xc)):
norm = l2norm(array([xc[i],yc[i],zc[i]])) - 1.0
res.append(norm)
return array(res)
def errfunc(pars):
return data - func(pars) #return the error
# some pseudo data; add some noise
data = func(parsTrue) + normal(0.0, 0.1, distance.shape)
# the intial guess of the params
guess = 1.0, -.4, 0.0
# now solve for the best fit paramters
best, info, ier, mesg = leastsq(errfunc, guess, full_output=1)
print 'true', parsTrue
print 'best', best
print '|err|_l2 =', P.l2norm(parsTrue - best)
# scipy's splrep uses FITPACK's curfit (B-spline interpolation)
print 'Spline smoothing of the data'
sp = splrep(distance, data)
smooth = splev(distance, sp)
print 'Spline information (see splrep and splev for details):', sp
# Now use pylab to plot
P.figure()
P.plot(distance, data, label='Noisy data')
P.plot(distance, func(best), lw=2, label='Best fit')
P.legend()
P.figure()
P.plot(distance, data, label='Noisy data')
P.plot(distance, smooth, lw=2, label='Spline-smoothing')
def processData(self):
print self.magSampleList
#plot(array(self.magSampleList))
#show()
self.mags = array(self.magSampleList)
xs = self.mags[:,0]
ys = self.mags[:,1]
zs = self.mags[:,2]
cxs, x_offset, x_scale = self.applyCalibration(xs)
cys, y_offset, y_scale = self.applyCalibration(ys)
czs, z_offset, z_scale = self.applyCalibration(zs)
def residuals(params,x,y,z):
xo = params[0]
xs = params[1]
yo = params[2]
ys = params[3]
zo = params[4]
zs = params[5]
xc = empty(shape(x))
for i in range(len(x)):
xc[i] = (x[i] - xo) * xs
yc = empty(shape(y))
for i in range(len(y)):
yc[i] = (y[i] - yo) * ys
zc = empty(shape(z))
for i in range(len(z)):
zc[i] = (z[i] - zo) * zs
res = []
for i in range(len(xc)):
norm = l2norm(array([xc[i],yc[i],zc[i]])) - 1.0
res.append(norm)
return array(res)
p0 = [x_offset, x_scale, y_offset, y_scale, z_offset, z_scale]
ls = leastsq(residuals, p0, args=(xs,ys,zs))
x_offset = ls[0][0]
x_scale = ls[0][1]
y_offset = ls[0][2]
y_scale = ls[0][3]
z_offset = ls[0][4]
z_scale = ls[0][5]
cxs = self.applyCalibration(xs, calibration=(x_offset,x_scale))[0]
cys = self.applyCalibration(ys, calibration=(y_offset,y_scale))[0]
czs = self.applyCalibration(zs, calibration=(z_offset,z_scale))[0]
calibratedMag = empty((len(cxs),3))
calibratedMag[:,0] = cxs
calibratedMag[:,1] = cys
calibratedMag[:,2] = czs
magnitudes = amap(lambda v: l2norm(v), calibratedMag)
self.calibratedMag = calibratedMag
mlab.points3d(cxs,cys,czs, scale_mode='none', scale_factor=0.02)
#mlab.points3d(array([-1.0,1.0]),array([0.0,0.0]),array([0.0,0.0]), scale_mode='none', scale_factor=0.02)
sphere = mlab.points3d(0,0,0, opacity=0.5, resolution=100, color=(1.0,0.0,0.0), scale_mode='none', scale_factor=2.0)
sphere.actor.property.backface_culling = True
mlab.show()
figure()
plot(magnitudes)
show()
# write calibration to a file
f = open(self.filename,mode='w')
f.write("id,x_offset,x_scale,y_offset,y_scale,z_offset,z_scale\n")
f.write("%d,%f,%f,%f,%f,%f,%f\n"%(self.id,x_offset,x_scale,y_offset,y_scale,z_offset,z_scale))
f.write("\n")
for l in self.magSampleList:
f.write("%d,%d,%d\n"%(l[0],l[1],l[2]))
f.close()
resError = []
for i in range(len(cxs)):
resError.append(self.calculateResidualError(cxs[i],cys[i],czs[i]))
print len(resError)
print mean(resError)
def calculateResidualError(self,xs,ys,zs):
errors = []
norm = l2norm(array([xs,ys,zs])) - 1.0
errors.append(norm * norm)
return mean(errors)
def errfunc(pars):
return data - func(pars) #return the error
# some pseudo data; add some noise
data = func(parsTrue) + normal(0.0, 0.1, distance.shape)
# the intial guess of the params
guess = 1.0, -.4, 0.0
# now solve for the best fit paramters
best, info, ier, mesg = leastsq(errfunc, guess, full_output=1)
print 'true', parsTrue
print 'best', best
print '|err|_l2 =',P.l2norm(parsTrue-best)
# scipy's splrep uses FITPACK's curfit (B-spline interpolation)
print 'Spline smoothing of the data'
sp = splrep(distance,data)
smooth = splev(distance,sp)
print 'Spline information (see splrep and splev for details):',sp
# Now use pylab to plot
P.figure()
P.plot(distance,data,label='Noisy data')
P.plot(distance,func(best),lw=2,label='Best fit')
P.legend()
P.figure()
P.plot(distance,data,label='Noisy data')
P.plot(distance,smooth,lw=2,label='Spline-smoothing')
def angle(a,b):
return arccos(dot(a,b) / (l2norm(a) * l2norm(b)))
def l2norm(self, v):
return pylab.l2norm(v)
