Example #1
0
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
Example #2
0
    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))
Example #3
0
 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))
Example #5
0
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
Example #6
0
 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
Example #8
0
 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))
Example #9
0
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
Example #11
0
 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])))
Example #12
0
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]
Example #13
0
    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)
Example #14
0
    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))
Example #15
0
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)
Example #16
0
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)
Example #17
0
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
Example #18
0
    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
Example #19
0
    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
Example #20
0
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)
Example #21
0
 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
Example #22
0
    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])
Example #23
0
 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()
Example #24
0
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
Example #25
0
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
Example #26
0
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]
Example #27
0
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)
Example #28
0
 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
Example #29
0
    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
Example #30
0
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
Example #31
0
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
Example #32
0
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]
Example #33
0
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
Example #34
0
 def test_sub_3d(self):
     v1 = [1, 2, 3]
     v2 = [4, 5, 6]
     n  = [-3, -3, -3]
     self.assertEqual(sub(v1, v2), n)
Example #35
0
 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]))
Example #36
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]
Example #37
0
 def test_sub_zero(self):
     v = (0, 0, 0)
     self.assertEqual(sub(v, v), v)
Example #38
0
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
Example #39
0
 def test_sub_3d(self):
     v1 = (1, 2, 3)
     v2 = (4, 5, 6)
     n  = (-3, -3, -3)
     self.assertEqual(sub(v1, v2), n)
Example #40
0
 def test_sub_2d(self):
     v1 = [1, 2]
     v2 = [3, 4]
     n  = [-2, -2]
     self.assertEqual(sub(v1, v2), n)
Example #41
0
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]
Example #42
0
 def ang(p):
     v = rotvecquat(vector.sub(p.compass, bias),
                    vec2vec2quat(p.down, [0, 0, 1]))
     return math.atan2(v[1], v[0])
Example #43
0
 def update(self, pos):
     self.p2 = add(sub(self.p2, self.p1), pos)
     self.p1 = pos
Example #44
0
 def test_sub_zero(self):
     v = [0, 0, 0]
     self.assertEqual(sub(v, v), v)
Example #45
0
 def test_sub_2d(self):
     v1 = (1, 2)
     v2 = (3, 4)
     n  = (-2, -2)
     self.assertEqual(sub(v1, v2), n)
Example #46
0
 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)