# How to use the vector.sub function in vector

## To help you get started, weβve selected a few vector examples, based on popular ways it is used in public projects.

Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.

pypilot / pypilot / pypilot / calibration_fit.py View on Github
def PointFit(points):
avg = AvgPoint(points)
dev = 0
max_dev = 0
for p in points:
v = vector.sub(p[:3], avg)
d = vector.dot(v, v)
max_dev = max(d, max_dev)
dev += d
dev /= len(points)
return avg, dev**.5, max_dev**.5
pypilot / pypilot / pypilot / calibration_fit.py View on Github
def FitAccel(debug, accel_cal):
p = accel_cal.Points()
if len(p) &lt; 5:
return False

mina = lmap(min, *p)
maxa = lmap(max, *p)
diff = vector.sub(maxa[:3], mina[:3])
if min(*diff) &lt; 1.2:
debug('need more range', min(*diff))
return # require sufficient range on all axes
if sum(diff) &lt; 4.5:
fit = FitPointsAccel(debug, p)

if abs(1-fit[3]) &gt; .1:
debug('scale factor out of range', fit)
return

dev = ComputeDeviation(p, fit)
return [fit, dev]
pypilot / pypilot / pypilot / calibration_fit.py View on Github
new_sphere1d_fit = lmap(lambda x, a: x + new_sphere1d_fit[0]*a, initial[:3], norm) + [new_sphere1d_fit[1], math.degrees(math.asin(new_sphere1d_fit[2]))]
new_sphere1d_fit = [new_sphere1d_fit, ComputeDeviation(points, new_sphere1d_fit), 1]
#print('new sphere1 fit', new_sphere1d_fit)

if line_max_dev &lt; 2:
debug('line fit found, insufficient data', line_dev, line_max_dev)
return False

# 2d sphere fit across normal vector
u = vector.cross(norm, [norm[1]-norm[2], norm[2]-norm[0], norm[0]-norm[1]])
v = vector.cross(norm, u)
u = vector.normalize(u)
v = vector.normalize(v)

# initial is the closest to guess on the uv plane containing current
initial = vector.add(guess[:3], vector.project(vector.sub(current[:3], guess[:3]), norm))
initial.append(current[3])
#debug('initial 2d fit', initial)

'''
def f_sphere2(beta, x):
bias = lmap(lambda x, a, b: x + beta[0]*a + beta[1]*b, initial[:3], u, v)
b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = lmap(lambda y : beta[2] - vector.norm(y), m)
return r0
sphere2d_fit = FitLeastSq([0, 0, initial[3]], f_sphere2, zpoints)
if not sphere2d_fit or sphere2d_fit[2] &lt; 0:
print('FitLeastSq sphere2d failed!!!! ', len(points))
new_sphere2d_fit = initial
else:
sphere2d_fit = lmap(lambda x, a, b: x + sphere2d_fit[0]*a + sphere2d_fit[1]*b, initial[:3], u, v) + [sphere2d_fit[2]]
pypilot / pypilot / pypilot / calibration_fit.py View on Github
def ang(p):
c = quaternion.rotvecquat(vector.sub(p[:3], bias), q)
d = quaternion.rotvecquat(p[3:6], q)
v = quaternion.rotvecquat(c, quaternion.vec2vec2quat(d, [0, 0, 1]))
v = vector.normalize(v)
return math.degrees(math.atan2(v[1], v[0]))
#, abs(math.degrees(math.acos(v[2])))
pypilot / pypilot / pypilot / calibration_fit.py View on Github
plane_fit, plane_dev, plane_max_dev = plane

# initial guess average min and max for bias, and average range for radius
minc = [1000, 1000, 1000]
maxc = [-1000, -1000, -1000]
for p in points:
minc = lmap(min, p[:3], minc)
maxc = lmap(max, p[:3], maxc)

guess = lmap(lambda a, b : (a+b)/2, minc, maxc)
diff = lmap(lambda a, b : b-a, minc, maxc)
guess.append((diff[0]+diff[1]+diff[2])/3)
#debug('initial guess', guess)

# initial is the closest to guess on the uv plane containing current
initial = vector.add(current[:3], vector.project(vector.sub(guess[:3], current[:3]), norm))
initial.append(current[3])
#debug('initial 1d fit', initial)

# attempt 'normal' fit along normal vector
'''
def f_sphere1(beta, x):
bias = lmap(lambda x, n: x + beta[0]*n, initial[:3], norm)
b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = lmap(lambda y : beta[1] - vector.norm(y), m)
return r0
sphere1d_fit = FitLeastSq([0, initial[3]], f_sphere1, zpoints)
if not sphere1d_fit or sphere1d_fit[1] &lt; 0:
print('FitLeastSq sphere1d failed!!!! ', len(points))
return False
sphere1d_fit = lmap(lambda x, n: x + sphere1d_fit[0]*n, initial[:3], norm) + [sphere1d_fit[1]]
pypilot / pypilot / pypilot / calibration_fit.py View on Github
def ComputeDeviation(points, fit):
m, d  = 0, 0
for p in points:
v = vector.sub(p[:3], fit[:3])
m += (1 - vector.dot(v, v) / fit[3]**2)**2

if len(fit) &gt; 4:
n = vector.dot(v, p[3:]) / vector.norm(v)
if abs(n) &lt;= 1:
ang = math.degrees(math.asin(n))
d += (fit[4] - ang)**2
else:
d += 1e111
m /= len(points)
d /= len(points)
return [m**.5, d**.5]
pypilot / pypilot / pypilot / calibration_fit.py View on Github
data = numpy.array(list(zip(zpoints[0], zpoints[1], zpoints[2])))
datamean = data.mean(axis=0)
uu, dd, vv = numpy.linalg.svd(data - datamean)

line_fit = [datamean, vv[0]]
plane_fit = [datamean, vv[2]]

line_dev = 0
max_line_dev = 0
plane_dev = 0
max_plane_dev = 0
for p in data:
t = vector.dot(p, line_fit[1]) - vector.dot(line_fit[0], line_fit[1])
q = lmap(lambda o, n : o + t*n, line_fit[0], line_fit[1])
v = vector.sub(p, q)
d = vector.dot(v, v)
max_line_dev = max(d, max_line_dev)
line_dev += d

t = vector.dot(p, plane_fit[1]) - vector.dot(plane_fit[0], plane_fit[1])
v = lmap(lambda b : t*b, plane_fit[1])
d = vector.dot(v, v)
max_plane_dev = max(d, max_plane_dev)
plane_dev += d

line_dev /= len(points)
plane_dev /= len(points)

line = [line_fit, line_dev**.5, max_line_dev**.5]
plane = [plane_fit, plane_dev**.5, max_plane_dev**.5]
return line, plane

## vector

Vector classes and utilities

BSD-3-Clause