def update_order(Vx,Vy,dt):
global t, v_init, has_scored, colli_point, START_POS, has_colli, score_point
if v_init[1] > 0:
if t > v_init[1]/abs(G): # is falling
ball.group = mid # change gourp
basket[1].group = front
#if check_collision() != None: # if collide
#reset_time()
#reset_ball()
#time.sleep(0.2)
if check_collision() == colli_point[0]:
has_colli = True
reset_time()
vec1 = vector.sub([ball.x, ball.y],colli_point[0])
vec1 = vector.vec_x_float(vec1,2)
v_init = vector.add(vec1,[Vx,Vy])
v_init = vector.vec_x_float(v_init,5)
START_POS[0] = ball.x
START_POS[1] = ball.y
elif check_collision() == colli_point[1]:
has_colli = True
reset_time()
vec1 = vector.sub([ball.x, ball.y],colli_point[1])
vec1 = vector.vec_x_float(vec1,2)
v_init = vector.add(vec1,[Vx,Vy])
v_init = vector.vec_x_float(v_init,5)
START_POS[0] = ball.x
START_POS[1] = ball.y
if vector.distance([ball.x,ball.y],score_point) <= 50:
has_scored = True
else:
ball.group = front
basket[1].group = mid
def separation(self, list):
mean = (0.0, 0.0)
for i in list:
dist = vector.mag(vector.sub(self.rect.center, i.rect.center))
if dist > 0 and dist < self.__separation_limit:
mean = vector.add(mean, vector.divs(vector.sub(self.rect.center, i.rect.center), dist))
return vector.divs(mean, len(list))
def can_see(self, other):
if vector.mag2(vector.sub(other.rect.center, self.rect.center)) <= self.__vis_radius ** 2:
angle = vector.dir(vector.sub(other.rect.center, self.rect.center)) - (
self.direction - self.__vis_angle / 2
) % math.radians(360)
if angle < self.__vis_angle:
return True
return False
def evaluate(self, goal, goal2):
dir = vector.normalize(vector.from_to(self.position, goal))
stop = vector.sub(self.velocity,
vector.mult(vector.along(self.velocity, dir), 0.25))
if vector.sqr_distance(self.position, goal) < 0.2:
return vector.normalize(
vector.sub(vector.sub(dir, stop), self.velocity))
return vector.normalize(vector.sub(dir, stop))
def check_collision():
global colli_point
vecd0 = vector.sub(colli_point[0],[ball.x, ball.y])
vecd1 = vector.sub(colli_point[1],[ball.x, ball.y])
if vector.distance(colli_point[0], [ball.x,ball.y]) <= 60:
return colli_point[0]
elif vector.distance(colli_point[1], [ball.x,ball.y]) <= 60:
return colli_point[1]
else:
return None
def slope(self, pos, ref_planet):
total = [0, 0] # Tracks the total acceleration on the planet
for planet in self.planet_list:
if (ref_planet != planet):
# Uses Newton's Law of Universial Gravitation to find the component of acceleration caused by each other planet in the system
total = vector.add(
total,
vector.scalarMult(
vector.sub(planet.nextpos, pos),
(planet.mass /
pow(vector.mag(vector.sub(planet.nextpos, pos)), 3))))
return (vector.scalarMult(total, G))
def batch_descent(X, y, alpha, w):
global logs
logs = []
alpha /= len(X)
for epoch in range(1, 1000):
loss = vector.sub(y, vector.mul_mat_vec(X, w))
gradient = vector.mul_mat_vec(vector.transpose(X), loss)
w_old = w
w = vector.add(w, vector.mul(alpha, gradient))
logs += (w, alpha, sse(X, y, w))
if vector.norm(vector.sub(w, w_old)) / vector.norm(w) < 1.0e-5:
break
print("Epoch", epoch)
return w
def draw(self, layer):
for y in range(11):
for x in range(15):
map_position = vector.sub((x, y), self.model.player.position)
sprite_id = layer[map_position[0]][map_position[1]]
sprite_image = self.model.sprites.sprites[sprite_id]
self.model.window.blit(sprite_image, vector.add((x * 32, y * 32), self.model.camera.offset))
def stoch_descent(X, y, alpha, w):
"""
Stochastic gradient descent
:param X:
:param y:
:param alpha:
:param w:
:return:
"""
global logs, logs_stoch
logs = []
logs_stoch = []
random.seed(0)
idx = list(range(len(X)))
for epoch in range(1000):
random.shuffle(idx)
w_old = w
for i in idx:
loss = y[i] - vector.dot(X[i], w)
gradient = vector.mul(loss, X[i])
w = vector.add(w, vector.mul(alpha, gradient))
logs_stoch += (w, alpha, sse(X, y, w))
if vector.norm(vector.sub(w, w_old)) / vector.norm(w) < 1.0e-5:
break
logs += (w, alpha, sse(X, y, w))
print("Epoch", epoch)
return w
def stoch_descent(X, y, alpha, w):
"""
Stochastic gradient descent
:param X:
:param y:
:param alpha:
:param w:
:return:
"""
global logs, logs_stoch
logs = []
logs_stoch = []
random.seed(0)
idx = list(range(len(X)))
for epoch in range(1000):
random.shuffle(idx)
w_old = w
for i in idx:
loss = y[i] - vector.dot(X[i], w)
gradient = vector.mul(loss, X[i])
w = vector.add(w, vector.mul(alpha, gradient))
logs_stoch += (w, alpha, sse(X, y, w))
if vector.norm(vector.sub(w, w_old)) / vector.norm(w) < 1.0e-5:
break
logs += (w, alpha, sse(X, y, w))
print("Epoch", epoch)
return w
def ang(p):
c = rotvecquat(vector.sub(p.compass, bias), q)
d = rotvecquat(p.down, q)
v = rotvecquat(c, 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])))
def FitAccel(debug, accel_cal):
p = accel_cal.Points()
if len(p) < 5:
return False
mina = lmap(min, *p)
maxa = lmap(max, *p)
diff = vector.sub(maxa[:3], mina[:3])
if min(*diff) < 1.2:
debug('need more range', min(*diff))
return # require sufficient range on all axes
if sum(diff) < 4.5:
debug('need more spread', sum(diff))
return # require more spread
fit = FitPointsAccel(debug, p)
if not fit:
debug('FitPointsAccel failed', fit)
return False
if abs(1-fit[3]) > .1:
debug('scale factor out of range', fit)
return
dev = ComputeDeviation(p, fit)
return [fit, dev]
def calculate_normal(self, b1, b2):
"""
Calculates the normal of b1 with respect to b2
"""
normal_x = 0.0
normal_y = 0.0
# Difference between body centers
center_dir = vector.normalize(vector.sub(b1.rect.center(), b2.rect.center()))
cos_threshold = b2.rect.w / math.sqrt(math.pow(b2.rect.h, 2) + math.pow(b2.rect.w, 2))
if vector.dot(center_dir, vector.Vector2(1, 0)) >= cos_threshold:
# b1 is at the right side of b2
normal_x = 1.0
elif vector.dot(center_dir, vector.Vector2(-1, 0)) >= cos_threshold:
# b1 is at the left side of b2
normal_x = -1.0
elif vector.dot(center_dir, vector.Vector2(0, -1)) >= math.cos(math.pi * 0.5 - math.acos(cos_threshold)):
# b1 is above b2
normal_y = -1.0
else:
# b1 is below b2
normal_y = 1.0
return vector.Vector2(normal_x, normal_y)
def bound(self, area):
v = (0, 0)
diff = area.left - self.rect.left
if diff > 0:
v = vector.add(v, (diff, 0))
diff = self.rect.right - area.right
if diff > 0:
v = vector.sub(v, (diff, 0))
diff = area.top - self.rect.top
if diff > 0:
v = vector.add(v, (0, diff))
diff = self.rect.bottom - area.bottom
if diff > 0:
v = vector.sub(v, (0, diff))
self.velocity = vector.add(self.velocity, vector.muls(v, self.__bound_factor))
def sse(X, y, w):
"""
Sum of squared errors
:param X:
:param y:
:param w:
:return:
"""
error = vector.sub(y, vector.mul_mat_vec(X, w))
return vector.dot(error, error)
def sse(X, y, w):
"""
Sum of squared errors
:param X:
:param y:
:param w:
:return:
"""
error = vector.sub(y, vector.mul_mat_vec(X, w))
return vector.dot(error, error)
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
def longBlit(self):
if self.attacking:
blit_pos = self.game.off(self.game.Player.getPos())
blit_pos = vector.add(blit_pos, self.game.Player.getRigging())
blit_pos = vector.sub(blit_pos, self.attack_image.get_size())
#offset for left/back
blit_pos[0] += self.x_offset
blit_pos[1] += self.y_offset
self.new_rect = self.game.screen.blit(self.mod_DAT, blit_pos)
self.new_rect.center = self.attack_rect.center
self.attack_rect = self.new_rect
def steer(self, v):
diff = vector.sub(v, self.rect.center)
dist = vector.mag(diff)
if dist > 0:
diff = vector.divs(diff, dist)
damp = 64.0
if dist < damp:
diff = vector.muls(diff, self.__velocity_max * (dist / damp))
else:
diff = vector.muls(diff, self.__velocity_max)
vec = vector.sub(diff, self.velocity)
vecdist = vector.mag(vec)
if vecdist > self.__force_max:
vec = vector.muls(vector.divs(vec, vecdist), self.__force_max)
else:
vec = (0, 0)
return vec
def update_enemy(position, malo_pos, delta_time, camera):
'''
Actualiza el estado de la calabaza
'''
vertex = [0, 0]
malo_velocity = mult(normalizar(sub(position, malo_pos)),
SPEED * 0.25 * delta_time)
orientacion = rotar.rot(camera, math.pi / 2)
malo_pos = add(malo_pos, malo_velocity)
vertex[0] = add(malo_pos, mult(orientacion, 12.5))
vertex[1] = add(malo_pos, mult(orientacion, -12.5))
return (vertex, malo_pos)
def train(self, training_input):
wrong_predictions = 0
for input, label in zip([item[1:] for item in training_input], [item[0] for item in training_input]):
prediction = self.predict(input)
if prediction != label:
wrong_predictions += 1
if label == 0 and prediction == 1:
self.weights[1:] = vector.sub(self.weights[1:], vector.mul(self.learning_rate, input))
self.weights[0] -= self.learning_rate
if label == 1 and prediction == 0:
self.weights[1:] = vector.add(vector.mul(self.learning_rate, input), self.weights[1:])
self.weights[0] += self.learning_rate
def predict(self, accel_ned_magnetic, t):
# log new prediction and apply it estimating new state
# subtract gravity
residual_accel_magnetic = vector.sub(accel_ned_magnetic, [0, 0, 1])
# rotate by declination
decl_q = quaternion.angvec2quat(self.declination.value, [0, 0, 1])
accel_true = quaternion.rotvecquat(residual_accel_magnetic, decl_q)
# apply predicted compass error
error_q = quaternion.angvec2quat(self.compass_offset.value, [0, 0, 1])
accel_ned = quaternion.rotvecquat(accel_true, error_q)
U = 9.81 * np.array(accel_ned)
if not self.use3d:
U = U[:2]
t = time.monotonic()
dt = t - self.predict_t
self.predict_t = t
if dt < 0 or dt > .5:
self.reset()
if not self.X: # do not have a trusted measurement yet, so cannot perform predictions
return
self.apply_prediction(dt, U)
self.history.append({
't': t,
'dt': dt,
'U': U,
'X': self.X,
'P': self.P
})
# filtered position
ll = xy_to_ll(self.X[0], self.X[1], *self.lastll)
self.lat.set(ll[0])
self.lon.set(ll[1])
# filtered speed and track
c = 3 if self.use3d else 2
vx = self.X[c]
vy = self.X[c + 1]
self.speed.set(math.hypot(vx, vy))
self.track.set(resolv(math.degrees(math.atan2(vx, vy)), 180))
# filtered altitude and climb
if self.use3d:
self.alt.set(self.X[2])
self.climb.set(self.X[5])
def update(self, pos):
global screen_offset_x, screen_offset_y, screen_dirty
offset = vec.sub(pos, self.start_pos)
if self.stage == 1:
if pg.time.get_ticks() - self.start_ticks >= DRAG_START_MILLIS:
self.stage = 2
elif offset[0]**2 + offset[1]**2 >= DRAG_START_PX**2:
self.stage = 2
if self.stage == 2:
(screen_offset_x,
screen_offset_y) = vec.add(self.start_view, offset)
screen_dirty = True
redraw()
def batch_descent(X, y, alpha, w):
"""
Batch gradient descent
:param X:
:param y:
:param alpha:
:param w:
:return:
"""
global logs
logs = []
alpha /= len(X)
for epoch in range(1, 1000):
loss = vector.sub(y, vector.mul_mat_vec(X, w))
gradient = vector.mul_mat_vec(vector.transpose(X), loss)
w_old = w
w = vector.add(w, vector.mul(alpha, gradient))
logs += (w, alpha, sse(X, y, w))
if vector.norm(vector.sub(w, w_old)) / vector.norm(w) < 1.0e-5:
break
print("Epoch", epoch)
return w
def SGD(users,items,record,speed,penalty): ''' Pu is the user's feature vector Qi is the item's feature vector e is the prediction error the default rate for each is 1 SGD will iterate update Pu and Qi based on the following: Pu <--- Pu + speed * (e * Qi - penalty * Pu) Qi <--- Qi + speed * (e * Pu - penalty * Qi) ''' Pu = users[record[0]]['feature'] Qi = items[record[1]]['feature'] e = float(record[2]) - vector.dot(Pu,Qi) pu = vector.plus(Pu , vector.num_dot(speed , vector.sub(vector.num_dot(e , Qi) , vector.num_dot(penalty , Pu) ) ) ) Qi = vector.plus(Qi , vector.num_dot(speed , vector.sub(vector.num_dot(e , Pu) , vector.num_dot(penalty , Qi) ) ) ) Pu = pu users[record[0]]['feature'] = Pu items[record[1]]['feature'] = Qi
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) > 4:
n = vector.dot(v, p[3:]) / vector.norm(v)
if abs(n) <= 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]
def hadle_events(rayos, delta_time, objetos, modo, en_creacion, position,
camera):
'''
Manejador de eventos
'''
en_creacion = en_creacion
rayos = rayos
running = True
keys = pygame.key.get_pressed()
if keys[pygame.K_LEFT]:
camera = rotar.rot(camera, -ANGLE_SPEED * delta_time)
rayos = calcular_rayos(camera)
elif keys[pygame.K_RIGHT]:
#OPTIMAZACION ROTAR RAYOS SOLO CUANDO ROTA LA CAMARA
camera = rotar.rot(camera, ANGLE_SPEED * delta_time)
rayos = calcular_rayos(camera)
elif keys[pygame.K_UP]:
position = add(position, mult(camera, SPEED * delta_time))
elif keys[pygame.K_DOWN]:
position = sub(position, mult(camera, SPEED * delta_time))
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
if pygame.MOUSEBUTTONDOWN:
if pygame.mouse.get_pressed()[0]:
if not en_creacion:
en_creacion = {
"vertex": [pygame.mouse.get_pos(), 0],
"color": (100, 100, 255),
"textured": False
}
else:
en_creacion["vertex"][1] = pygame.mouse.get_pos()
en_creacion["color"] = (random.randint(0, 255),
random.randint(0, 255),
random.randint(0, 255))
objetos.append(en_creacion.copy())
en_creacion = 0
print("Posicion", pygame.mouse.get_pos())
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_w:
modo = not modo
return (modo, running, rayos, en_creacion, position, camera)
def should_capture(self, pos):
delta = vec.sub(pos, self.pos)
if vec.measure(delta) > 1:
return self.should_navigate(pos)
if not self.charge(cd_move):
return True
for x in get_tile(pos).contents:
if isinstance(x, Castle):
castle = x
break
else:
out("I'm sorry Mario, but your castle is in another castle! (This is a bug)"
)
return False
if castle.team == self.team and castle.unit_type == self.__class__:
self.ai.watch(pos)
return True
self.has_task(tasks.Capture(self, castle))
return True
def should_navigate(self, pos):
delta = vec.sub(pos, self.pos)
angles = vec.calc_angles(delta, self.bias_flip)
self.bias_angle = angles[0]
# Charge first before checking destinations;
# some obstructions (notably move claim tokens) are temporary, and we don't want to report a failure
# until we're actually ready to move and the obstacle is still there.
if not self.charge(cd_move):
return True
watchables = []
for angle in angles:
ret = self.try_move(angle)
if ret == True:
return True
else:
watchables += ret
if watchables:
for w in watchables:
self.ai.watch(w)
return True
return False
def LinearFit(points):
zpoints = [[], [], []]
for i in range(3):
zpoints[i] = map(lambda x: x[i], points)
data = numpy.array(list(zip(zpoints[0], zpoints[1], zpoints[2])))
print('zpoints', zpoints[0])
print('data', data)
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 = map(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 = list(map(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
def find_grid(img, cam_rot_guess):
"""
Takes an undistorted image and a camera rotation guess and finds the position
of the camera relative to the grid, as well as identifying all visible
squares in the grid.
:param img: The image to process
:param cam_rot_guess:
:return: (squares, cam_rot, base_object_points)
"""
birds_view = np.zeros([1000, 1000, 3], dtype=np.uint8)
cv2.circle(birds_view,
(int(birds_view.shape[0] / 2), int(birds_view.shape[1] / 2)), 5,
(255, 255, 0), -1)
squares, cam_rot_guess = grid.get_square_stats(img, camera_matrix,
np.array([[]]),
cam_rot_guess)
cam_rot = cam_rot_guess
square_length = 28.5
square_gap = 2
base_object_points = [[-square_length / 2, -square_length / 2, 0],
[-square_length / 2, square_length / 2, 0],
[square_length / 2, square_length / 2, 0],
[square_length / 2, -square_length / 2, 0]]
for square in squares:
temp_vec = vec.sub(square.location, squares[0].location)
temp_vec[0] = (square_length + square_gap) * round(
temp_vec[0] / (square_length + square_gap), 0)
temp_vec[1] = (square_length + square_gap) * round(
temp_vec[1] / (square_length + square_gap), 0)
temp_vec[2] = 0
# Where the magic happens. Gets vector from camera to center of square
return squares, cam_rot, base_object_points
def FitAccel(accel_cal):
p = accel_cal.Points()
debug('accelfit count', len(p))
if len(p) < 5:
return False
mina = map(min, *p)
maxa = map(max, *p)
diff = vector.sub(maxa[:3], mina[:3])
#print('accelfit', diff)
if min(*diff) < 1.2:
return # require sufficient range on all axes
if sum(diff) < 4.5:
return # require more spread
fit = FitPointsAccel(p)
if abs(1-fit[3]) > .1:
debug('scale factor out of range', fit)
return
dev = ComputeDeviation(p, fit)
return [fit, dev]
def fit_batch(X, y, alpha, w, epochs=500, epsilon=1.0e-5):
"""
Batch gradient descent
:param X:
:param y:
:param alpha:
:param w:
:param epochs:
:param epsilon:
:return:
"""
global logs
logs = []
alpha /= len(X)
for epoch in range(epochs):
y_hat = predict(X, w)
loss = vector.sub(y, y_hat)
gradient = vector.mul_mat_vec(vector.transpose(X), loss)
w = vector.add(w, vector.mul(alpha, gradient))
logs += (w, alpha, sse(X, y, w))
if vector.norm(gradient) < epsilon:
break
print("Epoch", epoch)
return w
def test_sub_3d(self):
v1 = [1, 2, 3]
v2 = [4, 5, 6]
n = [-3, -3, -3]
self.assertEqual(sub(v1, v2), n)
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]))
def FitPointsCompass(debug, points, current, norm):
# ensure current and norm are float
current = lmap(float, current)
norm = lmap(float, norm)
zpoints = [[], [], [], [], [], []]
for i in range(6):
zpoints[i] = lmap(lambda x : x[i], points)
# determine if we have 0D, 1D, 2D, or 3D set of points
point_fit, point_dev, point_max_dev = PointFit(points)
if point_max_dev < 9:
debug('0d fit, insufficient data %.1f %.1f < 9' % (point_dev, point_max_dev))
return False
line, plane = LinearFit(points)
line_fit, line_dev, line_max_dev = line
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] < 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]]
debug('sphere1 fit', sphere1d_fit, ComputeDeviation(points, sphere1d_fit))
'''
def f_new_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)
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
fac = 1 # weight deviation
def dip(y, z):
n = min(max(vector.dot(y, z)/vector.norm(y), -1), 1)
return n
r1 = lmap(lambda y, z : fac*beta[1]*(beta[2]-dip(y, z)), m, g)
return r0 + r1
new_sphere1d_fit = FitLeastSq([0, initial[3], 0], f_new_sphere1, zpoints, debug, 2)
if not new_sphere1d_fit or new_sphere1d_fit[1] < 0 or abs(new_sphere1d_fit[2]) > 1:
debug('FitLeastSq new_sphere1 failed!!!! ', len(points), new_sphere1d_fit)
new_sphere1d_fit = current
else:
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 < 2:
debug('line fit found, insufficient data %.1f %.1f' % (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] < 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]]
debug('sphere2 fit', sphere2d_fit, ComputeDeviation(points, sphere2d_fit))
'''
def f_new_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)
#r0 = lmap(lambda y : 1 - vector.norm(y)/beta[2], m)
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
fac = 1 # weight deviation
def dip(y, z):
n = min(max(vector.dot(y, z)/vector.norm(y), -1), 1)
return n
r1 = lmap(lambda y, z : fac*beta[2]*(beta[3]-dip(y, z)), m, g)
return r0 + r1
new_sphere2d_fit = FitLeastSq([0, 0, initial[3], 0], f_new_sphere2, zpoints, debug, 2)
if not new_sphere2d_fit or new_sphere2d_fit[2] < 0 or abs(new_sphere2d_fit[3]) >= 1:
debug('FitLeastSq sphere2 failed!!!! ', len(points), new_sphere2d_fit)
return False
new_sphere2d_fit = lmap(lambda x, a, b: x + new_sphere2d_fit[0]*a + new_sphere2d_fit[1]*b, initial[:3], u, v) + [new_sphere2d_fit[2], math.degrees(math.asin(new_sphere2d_fit[3]))]
new_sphere2d_fit = [new_sphere2d_fit, ComputeDeviation(points, new_sphere2d_fit), 2]
if plane_max_dev < 1.2:
ang = math.degrees(math.asin(vector.norm(vector.cross(plane_fit[1], norm))))
debug('plane fit found, 2D fit only', ang, plane_fit, plane_dev, plane_max_dev)
if ang > 30:
debug('angle of plane not aligned to normal: no 2d fit')
new_sphere2d_fit = False
return [new_sphere1d_fit, new_sphere2d_fit, False]
# ok to use best guess for 3d fit
initial = guess
'''
def f_sphere3(beta, x):
bias = beta[:3]
b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = lmap(lambda y : beta[3] - vector.norm(y), m)
return r0
sphere3d_fit = FitLeastSq(initial[:4], f_sphere3, zpoints)
if not sphere3d_fit or sphere3d_fit[3] < 0:
print('FitLeastSq sphere failed!!!! ', len(points))
return False
debug('sphere3 fit', sphere3d_fit, ComputeDeviation(points, sphere3d_fit))
'''
def f_new_sphere3(beta, x):
b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], beta[:3]))
m = list(numpy.array(b.transpose()))
r0 = lmap(lambda y : beta[3] - vector.norm(y), m)
#r0 = lmap(lambda y : 1 - vector.norm(y)/beta[3], m)
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
fac = 1 # weight deviation
def dip(y, z):
n = min(max(vector.dot(y, z)/vector.norm(y), -1), 1)
return n
r1 = lmap(lambda y, z : fac*beta[3]*(beta[4]-dip(y, z)), m, g)
return r0 + r1
new_sphere3d_fit = FitLeastSq(initial[:4] + [0], f_new_sphere3, zpoints, debug, 2)
if not new_sphere3d_fit or new_sphere3d_fit[3] < 0 or abs(new_sphere3d_fit[4]) >= 1:
debug('FitLeastSq sphere3 failed!!!! ', len(points))
return False
new_sphere3d_fit[4] = math.degrees(math.asin(new_sphere3d_fit[4]))
new_sphere3d_fit = [new_sphere3d_fit, ComputeDeviation(points, new_sphere3d_fit), 3]
#debug('new sphere3 fit', new_sphere3d_fit)
return [new_sphere1d_fit, new_sphere2d_fit, new_sphere3d_fit]
def test_sub_zero(self):
v = (0, 0, 0)
self.assertEqual(sub(v, v), v)
def find_grid(img, cam_rot_guess):
"""
Takes an undistorted image and a camera rotation guess and finds the position
of the camera relative to the grid, as well as identifying all visible
squares in the grid.
:param img: The image to process
:param cam_rot_guess:
:return: (squares, cam_rot, base_object_points)
"""
birds_view = np.zeros([1000, 1000, 3], dtype=np.uint8)
cv2.circle(birds_view,
(int(birds_view.shape[0] / 2), int(birds_view.shape[1] / 2)), 5,
(255, 255, 0), -1)
squares, cam_rot_guess = grid.get_square_stats(img, camera_matrix,
np.array([[]]),
cam_rot_guess)
cam_rot = cam_rot_guess
glued_square_corners = []
glued_square_coords = []
square_length = 28.5
square_gap = 2
base_object_points = [[-square_length / 2, -square_length / 2, 0],
[-square_length / 2, square_length / 2, 0],
[square_length / 2, square_length / 2, 0],
[square_length / 2, -square_length / 2, 0]]
for square in squares:
temp_vec = vec.sub(square.location, squares[0].location)
temp_vec[0] = (square_length + square_gap) * round(
temp_vec[0] / (square_length + square_gap), 0)
temp_vec[1] = (square_length + square_gap) * round(
temp_vec[1] / (square_length + square_gap), 0)
temp_vec[2] = 0
for i in base_object_points:
glued_square_coords.append(
[[vec.add(i, temp_vec)[0]], [vec.add(i, temp_vec)[1]],
[vec.add(i, temp_vec)[2]]])
for i in vec.denumpify(square.corners):
glued_square_corners.append([[i[0]], [i[1]]])
if len(squares) > 0:
glued_square_corners = np.asarray(glued_square_corners).astype(float)
glued_square_coords = np.asarray(glued_square_coords).astype(float)
glued_square_corners.reshape(len(glued_square_corners), 2, 1)
glued_square_coords.reshape(len(glued_square_coords), 3, 1)
# Where the magic happens. Gets vector from camera to center of square
inliers, full_r_vec, full_t_vec = cv2.solvePnP(glued_square_coords,
glued_square_corners,
camera_matrix,
distortion_coefficients,
squares[0].rvec,
squares[0].tvec, True)
rot_matrix = cv2.Rodrigues(full_r_vec)
camera_pos = np.multiply(cv2.transpose(rot_matrix[0]), -1).dot(
full_t_vec)
cam_to_grid_transform = np.concatenate(
(cv2.transpose(rot_matrix[0]), camera_pos), axis=1)
cam_rot = list(cam_to_grid_transform.dot(np.array([0, 0, 1, 0])))
return squares, cam_rot, base_object_points
def test_sub_3d(self):
v1 = (1, 2, 3)
v2 = (4, 5, 6)
n = (-3, -3, -3)
self.assertEqual(sub(v1, v2), n)
def test_sub_2d(self):
v1 = [1, 2]
v2 = [3, 4]
n = [-2, -2]
self.assertEqual(sub(v1, v2), n)
def FitPoints(points, current, norm):
if len(points) < 5:
return False
# ensure current and norm are float
current = map(float, current)
norm = map(float, norm)
zpoints = [[], [], [], [], [], []]
for i in range(6):
zpoints[i] = map(lambda x: x[i], points)
# determine if we have 0D, 1D, 2D, or 3D set of points
point_fit, point_dev, point_max_dev = PointFit(points)
if point_max_dev < 9:
if debug:
print '0d fit, insufficient data', point_dev, point_max_dev, '< 9'
return False
line, plane = LinearFit(points)
line_fit, line_dev, line_max_dev = line
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 = map(min, p[:3], minc)
maxc = map(max, p[:3], maxc)
guess = map(lambda a, b: (a + b) / 2, minc, maxc)
diff = map(lambda a, b: b - a, minc, maxc)
guess.append((diff[0] + diff[1] + diff[2]) / 3)
if debug:
print '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])
if debug:
print 'initial 1d fit', initial
# attempt 'normal' fit along normal vector
'''
def f_sphere1(beta, x):
bias = map(lambda x, n: x + beta[0]*n, initial[:3], norm)
b = numpy.matrix(map(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = map(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] < 0:
print 'FitLeastSq sphere1d failed!!!! ', len(points)
return False
sphere1d_fit = map(lambda x, n: x + sphere1d_fit[0]*n, initial[:3], norm) + [sphere1d_fit[1]]
if debug:
print 'sphere1 fit', sphere1d_fit, ComputeDeviation(points, sphere1d_fit)
'''
def f_new_sphere1(beta, x):
bias = map(lambda x, n: x + beta[0] * n, initial[:3], norm)
b = numpy.matrix(map(lambda a, b: a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = map(lambda y: beta[1] - vector.norm(y), m)
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
fac = .03 # weight deviation as 1 degree ~ .03 mag
r1 = map(
lambda y, z: fac * beta[1] * (beta[2] - math.degrees(
math.asin(vector.dot(y, z) / vector.norm(y)))), m, g)
return r0 + r1
new_sphere1d_fit = FitLeastSq([0, initial[3], 0], f_new_sphere1, zpoints,
2)
if not new_sphere1d_fit or new_sphere1d_fit[1] < 0:
if debug:
print 'FitLeastSq new_sphere1 failed!!!! ', len(points)
new_sphere1d_fit = current
else:
new_sphere1d_fit = map(lambda x, a: x + new_sphere1d_fit[0] * a,
initial[:3], norm) + new_sphere1d_fit[1:]
new_sphere1d_fit = [
new_sphere1d_fit,
ComputeDeviation(points, new_sphere1d_fit), 1
]
#print 'new sphere1 fit', new_sphere1d_fit
if line_max_dev < 3:
if debug:
print 'line fit found, insufficient data', line_dev, line_max_dev
return [new_sphere1d_fit, False, 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])
if debug:
print 'initial 2d fit', initial
'''
def f_sphere2(beta, x):
bias = map(lambda x, a, b: x + beta[0]*a + beta[1]*b, initial[:3], u, v)
b = numpy.matrix(map(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = map(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] < 0:
print 'FitLeastSq sphere2d failed!!!! ', len(points)
new_sphere2d_fit = initial
else:
sphere2d_fit = map(lambda x, a, b: x + sphere2d_fit[0]*a + sphere2d_fit[1]*b, initial[:3], u, v) + [sphere2d_fit[2]]
if debug:
print 'sphere2 fit', sphere2d_fit, ComputeDeviation(points, sphere2d_fit)
'''
def f_new_sphere2(beta, x):
bias = map(lambda x, a, b: x + beta[0] * a + beta[1] * b, initial[:3],
u, v)
b = numpy.matrix(map(lambda a, b: a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = map(lambda y: beta[2] - vector.norm(y), m)
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
fac = .03 # weight deviation as 1 degree ~ .03 mag
r1 = map(
lambda y, z: fac * beta[2] * (beta[3] - math.degrees(
math.asin(vector.dot(y, z) / vector.norm(y)))), m, g)
return r0 + r1
new_sphere2d_fit = FitLeastSq([0, 0, initial[3], 0], f_new_sphere2,
zpoints, 2)
if not new_sphere2d_fit or new_sphere2d_fit[2] < 0:
if debug:
print 'FitLeastSq sphere failed!!!! ', len(points)
return False
new_sphere2d_fit = map(
lambda x, a, b: x + new_sphere2d_fit[0] * a + new_sphere2d_fit[1] * b,
initial[:3], u, v) + new_sphere2d_fit[2:]
new_sphere2d_fit = [
new_sphere2d_fit,
ComputeDeviation(points, new_sphere2d_fit), 2
]
#print 'new sphere2 fit', new_sphere2d_fit
if plane_max_dev < 1.2:
if debug:
print 'plane fit found, 2D fit only', plane_fit, plane_dev, plane_max_dev
return [new_sphere1d_fit, new_sphere2d_fit, False]
# ok to use best guess for 3d fit
initial = guess
'''
def f_sphere3(beta, x):
bias = beta[:3]
b = numpy.matrix(map(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = map(lambda y : beta[3] - vector.norm(y), m)
return r0
sphere3d_fit = FitLeastSq(initial[:4], f_sphere3, zpoints)
if not sphere3d_fit or sphere3d_fit[3] < 0:
print 'FitLeastSq sphere failed!!!! ', len(points)
return False
if debug:
print 'sphere3 fit', sphere3d_fit, ComputeDeviation(points, sphere3d_fit)
'''
def f_new_sphere3(beta, x):
b = numpy.matrix(map(lambda a, b: a - b, x[:3], beta[:3]))
m = list(numpy.array(b.transpose()))
r0 = map(lambda y: beta[3] - vector.norm(y), m)
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
fac = .03 # weight deviation as 1 degree ~ .03 mag
r1 = map(
lambda y, z: fac * beta[3] * (beta[4] - math.degrees(
math.asin(vector.dot(y, z) / vector.norm(y)))), m, g)
return r0 + r1
new_sphere3d_fit = FitLeastSq(initial[:4] + [0], f_new_sphere3, zpoints, 2)
if not new_sphere3d_fit or new_sphere3d_fit[3] < 0:
if debug:
print 'FitLeastSq sphere failed!!!! ', len(points)
return False
new_sphere3d_fit = [
new_sphere3d_fit,
ComputeDeviation(points, new_sphere3d_fit), 3
]
#print 'new sphere3 fit', new_sphere3d_fit
return [new_sphere1d_fit, new_sphere2d_fit, new_sphere3d_fit]
def ang(p):
v = rotvecquat(vector.sub(p.compass, bias),
vec2vec2quat(p.down, [0, 0, 1]))
return math.atan2(v[1], v[0])
def update(self, pos):
self.p2 = add(sub(self.p2, self.p1), pos)
self.p1 = pos
def test_sub_zero(self):
v = [0, 0, 0]
self.assertEqual(sub(v, v), v)
def test_sub_2d(self):
v1 = (1, 2)
v2 = (3, 4)
n = (-2, -2)
self.assertEqual(sub(v1, v2), n)
def draw(self, surface, color='black', width=1):
towards = add(self.p1, mul(sub(self.p2, self.p1), Ray.AMPLIFIER_DRAW))
pygame.draw.line(surface, pygame.Color(color), self.p1, towards, width)
def sse(X, y, w):
error = vector.sub(y, vector.mul_mat_vec(X, w))
return vector.dot(error, error)
