Example #1
0
    def search(self, process, search_orientation=[0]):
        if process['ARtag_found'] == False:
            ar_sub = rospy.Subscriber("ar_pose_marker", AlvarMarkers,
                                      self.ar_callback)
            rospy.wait_for_message("ar_pose_marker", AlvarMarkers)
        if process['contour_found'][1] == False:
            image_sub = rospy.Subscriber("camera/rgb/image_raw", Image,
                                         self.shape_cam_callback)
            rospy.wait_for_message("camera/rgb/image_raw", Image)

        for angle in search_orientation:
            if angle != 0:
                util.rotate(angle)
            if process['ARtag_found'] == False and self.search_ARtag():
                util.signal(1, onColor=Led.GREEN)
                process['ARtag_found'] = True

            if process['contour_found'][1] == False and self.search_contour(
                    process):
                util.signal(1, onColor=Led.ORANGE)
                process['contour_found'][1] = True

            if process['ARtag_found'] and process['contour_found'][1]:
                break

        #unregister

        if process['spot_id'] == process['unmarked_spot_id'][0]:
            util.signal(1, onColor=Led.RED)
            process['unmarked_spot_id'][1] = True
        return process
def g_christofides(g, exact_algo=held_karp, vc_algo=vertex_cover, beta=1.0):
    random.seed(0)
    asymm_edges = find_aymm_edges(g, beta)
    asymm_nodes = vc_algo(g.edge_subgraph(asymm_edges).to_undirected())
    symm_nodes = g.nodes() - asymm_nodes

    if len(symm_nodes) <= 1:
        return {**exact_algo(g), 'kernel_size': len(g)}
    elif len(asymm_nodes) <= 1:  # if the asymmetric tour is trivial, don't bother
        tour = christofides(g.to_undirected()) + list(asymm_nodes)
        tour = metric_shortcut(tour)
        return {'cost': tour_cost(tour, g), 'tour': tour, 'kernel_size': 0}

    double_node = random.choice(tuple(symm_nodes))
    asymm_nodes.add(double_node)

    asymm_tour = exact_algo(g.subgraph(asymm_nodes))['tour']
    undir_g = to_undirected(g.subgraph(symm_nodes))
    symm_tour = christofides(undir_g)
    rev_symm_tour = symm_tour[::-1]
    symm_tour = symm_tour if tour_cost(symm_tour, g) <= tour_cost(rev_symm_tour, g) else rev_symm_tour

    asymm_tour = rotate(asymm_tour, asymm_tour.index(double_node) + 1)  # double node at end
    symm_tour = rotate(symm_tour, symm_tour.index(double_node))  # double node at start
    tour = metric_shortcut(asymm_tour + symm_tour)

    return {'cost': tour_cost(tour, g), 'tour': tour, 'kernel_size': len(asymm_nodes)}
Example #3
0
    def execute(self, userdata):
        global stop, turn, twist_pub, current_work, unmarked_spot_id, shape_at_loc2, g_red_line_count, work4_returned

        if work4_returned and turn:
            turn = False
            work4_returned = False
        if turn:
            return 'turning'
        if not stop:
            twist_pub.publish(current_twist)
            return 'running'
        else:
            twist = Twist()
            twist_pub.publish(twist)
            rospy.sleep(0.5)

            #off ramp
            if g_red_line_count == 2:
                work4_returned = True
                g_red_line_count += 1
                tmp_time = time.time()
                while time.time() - tmp_time < 2:
                    twist_pub.publish(current_twist)
                twist_pub.publish(Twist())
                rotate(-35)
                tmp_time = time.time()
                while time.time() - tmp_time < 1.6:
                    twist_pub.publish(current_twist)
                twist_pub.publish(Twist())
                userdata.contour = {"shape_at_loc2":shape_at_loc2}
                return 'work4'

            return 'end'
Example #4
0
def main():
    root = Tk()
    root.title('Rotating line')

    canvas = Canvas(root, width=W, height=H, background='black')
    canvas.grid(row=0, column=1)

    lines = [random_line() for line in range(N_LINES)]
    colors = [
        make_discrete_color_series(random_color(), random_color(), N_STEPS)
        for color in range(N_LINES)
    ]
    centers = [random_point() for line in range(N_LINES)]

    for i in range(N_STEPS):
        for j in range(len(lines)):
            lines[j] = rotate(lines[j], DEG_INCR1, centers[j])
            lines[j] = rotate(lines[j], (j + 2) * DEG_INCR2,
                              (lines[j][0] + lines[j][1]) * 0.5)
            canvas.create_line(lines[j][0].as_list(),
                               lines[j][1].as_list(),
                               fill=colors[j][i])
        canvas.update()
        canvas.after(CYCLE_IN_MS)

    root.mainloop()
Example #5
0
    def target(self, x, t, x0):
        """ A directed sigmoid function target.

        Parameters
        ----------

        x: state
        t: time
        x0: initial state

        Returns
        -------

        (x, u, a)

        """
        pose0 = self.model.model_parameters(q=x0[:self.model.nd])
        xt0 = self.task.x(pose0)

        t0 = 1.0
        A = 0.3
        B = 4
        o = np.asmatrix([[0], [0]])
        xd, ud, ad = sigmoid(t, t0, A, B)

        xd = rotate(o, np.asmatrix([[xd], [0]]), self.angle)
        ud = rotate(o, np.asmatrix([[ud], [0]]), self.angle)
        ad = rotate(o, np.asmatrix([[ad], [0]]), self.angle)

        return (np.asmatrix(xt0 + xd),
                np.asmatrix(ud),
                np.asmatrix(ad))
Example #6
0
 def kill(self):
     new_size = self.__size / 2
     if new_size > self.MIN_SIZE:
         a1 = Asteroid(pos=np.copy(self.pos), move_direction=util.rotate(self.speed, -10),
                       size=new_size)
         a2 = Asteroid(pos=np.copy(self.pos), move_direction=util.rotate(self.speed, 10),
                       size=new_size)
         a1.add(self.groups())
         a2.add(self.groups())
     super(Asteroid, self).kill()
Example #7
0
    def rotateForward(self, numCharactersTyped):
        if numCharactersTyped % Config.setting('rightRotorRate') == 0:
            self.rightRotor = rotate(self.rightRotor, 1)

        if numCharactersTyped % Config.setting('middleRotorRate') == 0:
            self.middleRotor = rotate(self.middleRotor, 1)

        if numCharactersTyped % Config.setting('leftRotorRate') == 0:
            self.leftRotor = rotate(self.leftRotor, 1)

        self.setRotorWiring(self.leftRotor, self.middleRotor, self.rightRotor)
Example #8
0
def main():
    """
    main function to prepare data for Tiramisu algorithm
    """
    parser = argparse.ArgumentParser(
        description='reads image sets and augments the data for Tiramisu',
        prog='data_gen.py <args>')

    # Required arguments
    parser.add_argument("-i",
                        "--input",
                        required=True,
                        help="Path to image sets")
    parser.add_argument("-o",
                        "--output",
                        required=True,
                        help="Path to save test and train files")

    # Optional arguments
    parser.add_argument("-r",
                        "--ratio",
                        type=float,
                        default=0.2,
                        help="validation set ratio")

    # Creating required directories
    args = vars(parser.parse_args())
    if not os.path.exists(args['output'] + '/train/data/'):
        os.makedirs(args['output'] + '/train/data/')
    if not os.path.exists(args['output'] + '/validate/data/'):
        os.makedirs(args['output'] + '/validate/data/')
    if not os.path.exists(args['output'] + '/train/masks/'):
        os.makedirs(args['output'] + '/train/masks/')
    if not os.path.exists(args['output'] + '/validate/masks/'):
        os.makedirs(args['output'] + '/validate/masks/')
    if not os.path.exists(args['output'] + '/test/data/'):
        os.makedirs(args['output'] + '/test/data/')

    print("Creating an image per video...")
    combine(args['input'], args['output'])

    print("Generating a mask per video...")
    json_to_mask(args['input'], args['output'])

    print("augmenting the dataset...")
    slicer(args['output'])
    rotate(args['output'])
    transpose(args['output'])

    # Splitting the dataset into training and validation set
    split(args['output'], args['ratio'])
Example #9
0
 def execute(self, userdata):
     global g_tags, g_target_ARtag_id, g_cmd_vel_pub
     if rospy.is_shutdown():
         return 'end'
     else:
         if g_target_ARtag_id in g_tags:
             tag_pose = g_tags[g_target_ARtag_id]
             if tag_pose.position.x > 0.7:
                 #move with cmd_vel_pub
                 print(tag_pose.position.x)
                 twist = Twist()
                 twist.linear.x = 0.3
                 g_cmd_vel_pub.publish(twist)
                 return 'approach'
             else:
                 # # move with move base
                 # goal = MoveBaseGoal()
                 # goal.target_pose.header.frame_id = str(g_target_ARtag_id)
                 # goal.target_pose.pose.position = Point(0, 0, -0.7)
                 # goal.target_pose.pose.orientation = Quaternion(0, 0, 0, 1)
                 # self.client.send_goal(goal)
                 # self.client.wait_for_result()
                 util.rotate()
                 util.move(0.5, linear_scale=0.3)
                 util.rotate(-87)
                 util.move(1.2, linear_scale=0.3)
                 util.rotate(-87)
                 util.move(0.5, linear_scale=0.3)
                 util.rotate(-87)
                 return 'arrived'
         print("lost target")
         g_cmd_vel_pub.publish(Twist())
         return 'approach'
Example #10
0
    def __init__(self, left, middle, right, key3):
        # Keep track of the original positions
        self.originalLeftRotor = rotate([ch.upper() for ch in left],
                                        Config.setting('characters')[key3[0]])
        self.originalMiddleRotor = rotate(
            [ch.upper() for ch in middle],
            Config.setting('characters')[key3[1]])
        self.originalRightRotor = rotate([ch.upper() for ch in right],
                                         Config.setting('characters')[key3[2]])

        # Rotate these when encrypting/decrypting
        self.leftRotor = self.originalLeftRotor
        self.middleRotor = self.originalMiddleRotor
        self.rightRotor = self.originalRightRotor
        self.setRotorWiring(self.leftRotor, self.middleRotor, self.rightRotor)
Example #11
0
 def serial_compute_pair_distances(self):
   self.N_neighborhoods = len(self.Neighborhoods)
   self.N_pairs = self.N_neighborhoods*(self.N_neighborhoods-1)/2
   print "> computing", self.N_pairs,"distances ",
   every = max(4,int(self.N_pairs/10))
   self.Pair_distances = self.N_pairs*[0.]
   self.Pair_force_differences = self.N_pairs*[0.]
   self.dmin = util._big
   self.dmax = -1.
   k = 0
   self.Pairs = []
   f = open(self.dfile,"w")
   for i in range(0,self.N_neighborhoods):
     for j in range(i+1,self.N_neighborhoods):
       self.Pairs.append([i,j])
       self.PairMap[(i,j)] = k
       [d,R] = self.distance(self.Neighborhoods[i],self.Neighborhoods[j])
       self.Pair_distances[self.index(i,j)] = d
       fi = self.Forces[i]
       fj = self.Forces[j]
       df = util.error(fi,util.rotate(R,fj))
       self.Pair_force_differences[self.index(i,j)] = df
       print >>f, i,j,d,df,R[0][0],R[0][1],R[0][2], \
                           R[1][0],R[1][1],R[1][2], \
                           R[2][0],R[2][1],R[2][2]
       if (self.dmin > d) : self.dmin = d
       if (self.dmax < d) : self.dmax = d
       k += 1
       if (k%every==0) : sys.stdout.write("*")
       sys.stdout.flush()
   print " %.6f:%.6f"%(self.dmin,self.dmax),
   print " done"
   self.create_neighborhood_distances()
Example #12
0
 def getVision(self,x,y,face,theta,delta):
     orig = (x,y)
     point1 = util.rotate((x+delta,y),orig,face + (theta / 2))
     point2 = util.rotate((x+delta,y),orig,face - (theta / 2))
     
     minX = min(x,point1[0],point2[0])
     maxX = max(x,point1[0],point2[0])
     
     minY = min(y,point1[1],point2[1])
     maxY = max(y,point1[1],point2[1])
     items = []
     for xIndex in self.xRange(minX,maxX):
         for yIndex in self.yRange(minY,maxY):
             for i in self.gridcells[xIndex][yIndex]:
                 items.append(i)
     return set(items)
Example #13
0
    def protection_decision_algorithm(self):
        '''试图出现在进攻方想要出现的那一个格子上,更具体的说,以进攻方的目标格子为"目标",实行进攻方的策略.
        这个测试游戏里面敌舰只有一艘,所以敌人想要去的位置是确定的,如果敌人选择不动,以敌人的位置为目标位置'''
        '''得到敌人的目标位置'''
        enemy = self.env.enemy_ships[0]
        enemy_action = enemy.attack_decision_algorithm()
        if (enemy_action.stay):
            target = enemy.coordinate()
        else:
            enemy_direction = rotate(
                enemy.direction, enemy_action.angular_speed
                if enemy_action.clockwise else -enemy_action.angular_speed)

            enemy_x, enemy_y = enemy.coordinate()
            if (enemy_direction == 0.0):
                target = enemy_x - 1, enemy_y
            elif (enemy_direction == 90.0):
                target = enemy_x, enemy_y + 1
            elif (enemy_direction == 180.0):
                target = enemy_x + 1, enemy_y
            elif (enemy_direction == 270):
                target = enemy_x, enemy_y - 1

        return self.make_decision_by_nearest_position(enemy_x, enemy_y,
                                                      enemy_direction, target)
Example #14
0
def run(left: HSVImageData, right: HSVImageData):  # The actual thing
    print("- getting ready...")
    width = left.width  # Store the width and height of both images in easy-to-access variables because we verified earlier that both images are the same size.
    height = left.height

    similarity_maps = [
        [[0 for i in range(height)] for i in range(width)]
        for i in range(width)
    ]  # Create a bunch of empty greyscale images (aka 2D number arrays)

    print("- generating similarity maps...")
    for depth in range(
            width
    ):  # Depth is being interpreted in terms of a change in x-position, so we iterate horizontally.
        # We basically shift the image along to the right.
        for x in range(width):  # We iterate through the first image:
            for y in range(height):
                try:  # For each pixel, try and compare its values in the two images, incorporating the x-shift from above.
                    similarity_maps[depth][x][y] = similarity(
                        left[x + depth][y], right[x][y])
                except IndexError:  # Sometimes the image will be shifted too far and there won't be matching pairs where they don't overlap.
                    pass  # When this happens we just ignore it.

    similarity_maps = rotate(
        similarity_maps
    )  # This function just changes the 3D array of similarity maps to make it easier to iterate over in the way I want
    # It is kind of explained in util.py.

    print("- generating depth map...")
    depth_map = [
        [0 for j in range(height)] for i in range(width)
    ]  # Now we're making the final depth map, by selecting the depth at which the pixels are the most similar.
    # This is explained better in design.md.
    for x in range(
            width
    ):  # Iterate per-pixel through all the similarity maps at once.
        for y in range(height):

            max_depth = 0
            number_at_max = 0
            for (depth, sim) in enumerate(
                    similarity_maps[x][y]
            ):  # Find the depth with the highest similarity value for that pixel.
                if sim > similarity_maps[x][y][int(max_depth)]:
                    max_depth = int(depth)
                    number_at_max = 1

                elif sim == similarity_maps[x][y][int(
                        max_depth
                )]:  # If there's a tie for most similar, find the average of each of the most similar depths.
                    max_depth = int((max_depth * number_at_max) + depth)
                    number_at_max += 1
                    max_depth /= number_at_max

            depth_map[x][
                y] = max_depth  # Fill the value for that pixel in on the depth map.

    print("- done!")

    return depth_map  # finally return it
Example #15
0
    def move(self):
        #speed up/slowdown
        self.speed += util.lpfDelta(self.speed, 4 * random.random() - 2, 3)
        #rotate pi/4 in either direction
        self.face = util.lpfDelta(self.face,math.pi/2*random.random()-math.pi/4,10)
        self.face %= math.pi + math.pi

        cap = 10
        if self.speed > cap:
            self.speed = cap
        if self.speed < 0:
            self.speed = 0
        #update position
        point = util.rotate((self.x+self.speed,self.y),(self.x,self.y),self.face)
        dX = point[0]
        dY = point[1]
        self.x = util.lpfSingle(self.x, dX,5)
        self.y = util.lpfSingle(self.y, dY,5)

        #bounding box
        if self.x > Display.WIDTH:
            self.x = 0
        if self.x < 0:
            self.x = Display.WIDTH
        if self.y > Display.HEIGHT:
            self.y = 0
        if self.y < 0:
            self.y = Display.HEIGHT
Example #16
0
def find_tangent(LC_s, LC_t):
    """
  Given a source and target lighthouse center with radius 1, find the tangent. 
  
  This is done by first finding the angle between LC_t, LC_s, tang and then calculating the tang itself via rotation.
  """
    return rotate(LC_s, LC_t, np.arcsin(1 / dist_2d(LC_s, LC_t)))
Example #17
0
    def draw(i):
        state = "rotate"
        rotate = 0
        advance = 0
        print(i)
        for j in range(i):
            if state == "rotate":
                rotate += np.pi * 0.5 * 0.1
                if rotate >= np.pi * 0.5:
                    rotate = 0
                    state = "advance"
            elif state == "advance":
                advance = util.translate([0, advance], [0, -0.02])[1]
                finish = _translate_tile([0, 0],
                                         "d" * step,
                                         tile_size=tile_size)[1]
                if finish > advance:
                    advance = finish
                    state = "finish"
        if state == "finish":
            print("finish")

        t_tiles = util.rotate(tiles, rotate)
        t_tiles = util.translate(t_tiles, [0, advance])

        plt.cla()
        util.draw_circle()

        plt.plot(t_tiles[:, :, 0].T, t_tiles[:, :, 1].T, lw=0.5, color="blue")

        _plot_xy(marker, color="red", lw=1)
Example #18
0
 def set_timeperiods(self):
     """sets optimization periods based on selection of optimization hours
     in the dispatch configuration
     sets:
       period_hours = range from 1 to the number of opt_hours
       periods = range from 1 to the maximum number of periods (i.e. 8760/period_hours)
       period_timepoints = dictionary with keys of period and values of period hours
       period_flex_load_timepoints = dictionary with keys of period and values of a nested dictionary with the keys of period_hours and the values of those period hours offset
       by the flexible_load_constraint_offset configuration parameter
     """
     if hasattr(self, 'hours'):
         return
     self.num_hours = len(shape.shapes.active_dates_index)
     self.hours = range(self.num_hours)
     num_periods = int(round(self.num_hours / float(cfg.opt_period_length)))
     self.periods = range(num_periods)
     split_hours = [
         list(a) for a in np.array_split(self.hours, num_periods)
     ]  # splits into roughly equal lengths
     self.period_lengths = dict(
         zip(self.periods, [len(a) for a in split_hours]))
     self.period_timepoints = dict(zip(self.periods, split_hours))
     self.period_previous_timepoints = dict(
         zip(self.periods,
             [dict(zip(*(a, util.rotate(a, 1)))) for a in split_hours]))
     self.period_repeated = util.flatten_list([[p] * self.period_lengths[p]
                                               for p in self.periods])
Example #19
0
def resize_image(image: np.ndarray, width: int, height: int) -> np.ndarray:
    image_height, image_width, _ = image.shape
    if image_height > image_width and height > width:
        return cv2.resize(image, (width, height))
    if image_width > image_height and width > height:
        return cv2.resize(image, (width, height))
    image = util.rotate(image, 90)
    return cv2.resize(image, (width, height))
Example #20
0
def find_tangent(LL_s, LC_t):
    """
  Given a source point and target lighthouse center with radius 1, find the tangent. 
  
  This is done by first finding the angle between LC_t, LL_s, tang and then calculating the tang itself via rotation. LL is "Lighthouse->Left". 
  It is possible that the angle between the supposed 
  """
    return rotate(LL_s, LC_t, np.arcsin(1 / dist_2d(LL_s, LC_t)))
Example #21
0
def plot_images_and_angles(images, prediction, im=None, perrow=20, sort_by_confidence=True):
    N = len(images)

    if isinstance(prediction, tuple):
        angles, log_sigmasqs = prediction
    else:
        angles, log_sigmasqs = prediction, np.zeros_like(prediction) - 1.

    if sort_by_confidence:
        precisions = 1./np.exp(log_sigmasqs)
        perm = np.argsort(precisions)[::-1]
        images, angles, log_sigmasqs = images[perm], angles[perm], log_sigmasqs[perm]

    def plot_angle(ax, center, angle, sigmasq, line=None):
        precision = 1./sigmasq
        x0, y0 = center
        x1, y1 = x0 - precision*np.sin(-angle), y0 - precision*np.cos(-angle)
        if not line:
            ax.plot([x0, x1], [y0, y1], '-', color='cyan', linewidth=2)
        else:
            line.set_data([x0, x1], [y0, y1])

    make_pair = lambda im, angle: np.vstack((im, rotate(im, angle)))
    pairs = map(make_pair, images.reshape(-1, 30, 30), angles)

    if N > perrow:
        nrows = N // perrow
        pairs= iter(pairs)
        bigmat = np.vstack([np.hstack([pairs.next() for j in range(perrow)]) for i in range(nrows)])
    else:
        bigmat = np.hstack(pairs)

    if im is None:
        im = plt.matshow(bigmat, cmap='plasma')
    else:
        im.set_data(bigmat)
    ax = im.axes

    ax.autoscale(False)
    ax.axis('off')

    if N > perrow:
        xs = np.tile(-0.5 + 15 + 30*np.arange(N // nrows), nrows)
        ys = np.repeat(-0.5 + 15 + 60*np.arange(nrows), N // nrows)
    else:
        xs = -0.5 + 15 + 30*np.arange(N)
        ys = np.repeat(15, N)
    centers = zip(xs, ys)

    if not ax.lines:
        for center, angle, log_sigmasq in zip(centers, angles, log_sigmasqs):
            plot_angle(ax, center, angle, np.exp(log_sigmasq))
    else:
        for center, angle, log_sigmasq, line in zip(centers, angles, log_sigmasqs, ax.lines):
            plot_angle(ax, center, angle, np.exp(log_sigmasq), line)

    plt.draw()
    return im
Example #22
0
    def __add_in_distrurbance(self, t, tau, pose):
        """Adds distrubance to end effector.

        """
        angle = self.gamma
        o = np.asmatrix([[0], [0]])
        fd = np.asmatrix([[gaussian(t, self.a, self.t0, self.sigma)], [0]])
        fd = rotate(o, fd, angle)
        return tau + self.JtT.subs(pose) * fd
Example #23
0
	def rotate(self, grid, dA):
		if self.device:
			if self.device[0] == "2":
				dbase, ddir = self.device[:-1], int(self.device[-1])
				self.device = "%s%s" % (dbase, (ddir + dA) % 2)
			elif self.device[0] == "1":
				dbase, ddir = self.device[:-1], int(self.device[-1])
				self.device = "%s%s" % (dbase, (ddir + dA) % 4)
		return self.changecolors(grid, util.rotate(self.s, self.colors, dA))
Example #24
0
    def update_cursors(self, col):
        self.move_cursor(0, *(util.rotate([col[1]*self.size/2,0], col[0]*2*math.pi) + self.size/2))
        self.move_cursor(1, self.size+10, (1-col[2])*(self.size-20)+10)

        self.create_line(util.HSVtoRGB(*col))

        self.refresh(col[2])

        self.colour = list(col)
Example #25
0
 def execute(self, userdata):
     global g_odom, turn, work, current_work, twist_pub, on_additional_line
     #rotate(userdata.rotate_turns_in * 90, anglular_scale=1.0)
     rotate(userdata.rotate_turns_in * 85, anglular_scale=1.0)
     turn = False
     if work:
         if current_work == 1:
             return 'working1'
         elif current_work == 2:
             on_additional_line = True
             return 'working2'
         else:
             return 'working3'
     else:
         if on_additional_line:
             return 'working2'
         else:
             work = True
             return 'end'
    def execute(self, userdata):
        global g_twist_pub, g_full_red_line_count, current_twist, g_work4_returned

        if self.subscribered == False:
            self.subscribe()
            self.subscribered = True

        if self.loop_start_time == None:
            self.loop_start_time = time.time()

        if g_full_red_line_count == 2 and not g_work4_returned:
            twist = Twist()
            g_twist_pub.publish(twist)
            rospy.sleep(0.5)
            tmp_time = time.time()
            while time.time() - tmp_time < 1.85:
                g_twist_pub.publish(current_twist)
            g_twist_pub.publish(Twist())
            rotate(-35)
            tmp_time = time.time()
            while time.time() - tmp_time < 1.7:
                g_twist_pub.publish(current_twist)
            g_twist_pub.publish(Twist())
            
            self.usb_cam_subscriber.unregister()
            self.subscribered = False

            g_work4_returned = True
            return 'work4'

        else:
            if g_full_red_line_count == 3:
                g_work4_returned = False
            # start line
            if g_full_red_line_count == 4:
                g_full_red_line_count = 0
                g_twist_pub.publish(Twist())
                util.signal(2)
                self.loop_start_time = None

            g_twist_pub.publish(current_twist)
            return 'running'
Example #27
0
 def parallel_compute_pair_distances(self):
   k = 0
   self.Pairs = []
   q = mp.JoinableQueue()
   #q = mp.Queue()
   for i in range(0,self.N_neighborhoods):
     for j in range(i+1,self.N_neighborhoods):
       p = [i,j]
       self.Pairs.append(p)
       self.PairMap[(i,j)] = k
       k += 1
       q.put(p)
   self.N_pairs = len(self.Pairs)
   nprocs = mp.cpu_count()
   for i in range(nprocs):
     q.put(None)
   #r = mp.Queue()
   print "> computing", self.N_pairs,"distances with",nprocs,"processes"
   mgr = mp.Manager()
   r = mgr.dict()
   jobs = []
   for i in range(nprocs):
     jobs.append(mp.Process(target=self.distance_worker,args=(q,r,)))
   for job in jobs:
     job.start()
   q.close()
   print "> collecting ",
   for job in jobs:
     job.join()
   self.Pair_distances = self.N_pairs*[0.]
   self.Pair_force_differences = self.N_pairs*[0.]
   self.dmin = util._big
   self.dmax = -1.
   print len(r)," distances ",
   sys.stdout.flush()
   ds = dict(r) # necessary since the mgr version is oddly behaved
   f = open(self.dfile,"w")
   for idx in ds:
     i,j = self.pair(idx)
     d,R = ds[idx]
     self.Pair_distances[idx] = d
     fi = self.Forces[i]
     fj = self.Forces[j]
     df = util.error(fi,util.rotate(R,fj))
     self.Pair_force_differences[self.index(i,j)] = df
     print >>f, i,j,d,df,R[0][0],R[0][1],R[0][2], \
                         R[1][0],R[1][1],R[1][2], \
                         R[2][0],R[2][1],R[2][2]
     if (self.dmin > d) : self.dmin = d
     if (self.dmax < d) : self.dmax = d
   print " %.6f:%.6f"%(self.dmin,self.dmax),"done"
   sys.stdout.flush()
   f.close()
   self.create_neighborhood_distances()
Example #28
0
def byGreatestAngle(poly, q1, q2) -> list:
    angle = 0
    res = []
    for i in range(len(poly)):
        v1 = [q1[0] - poly[i][0], q1[1] - poly[i][1]]
        v2 = [q2[0] - poly[i][0], q2[1] - poly[i][1]]
        angb = angleBetween(v1, v2)
        if angb > angle and rotate(q1, q2, poly[i]) >= 0:
            angle = angb
            res = poly[i]
    return res
Example #29
0
 def draw(self):
     for lineSeg in self.drawPoints:
         Display.drawLine(lineSeg[0],lineSeg[1],lineSeg[2],lineSeg[3],(50,50,200))
     for lineSeg in self.drawPoints2:
         Display.drawLine(lineSeg[0],lineSeg[1],lineSeg[2],lineSeg[3],(50,200,50))
     self.drawPoints = []
     self.drawPoints2 = []
     Display.drawCircle(self.x,self.y,self.r,(0,0,0))
     Display.drawCircle(self.x,self.y,self.r-1,self.color)
     end = util.rotate((self.x+self.r,self.y),(self.x,self.y),self.face)
     Display.drawLine(self.x,self.y,end[0],end[1])
def test_transformation_rotation():
    for deg in range(-360, 360):
        unit = np.array([1, 0])
        euler_rotation = rotate(unit, deg)

        points = np.array([unit])

        rotation = Transformation(euler_angle=deg)
        matrix_rotation = rotation.transform(points)

        diff = euler_rotation - matrix_rotation
        assert(np.all(diff < 1e-10)), "difference between rotation angle and matrix = %r" % diff
Example #31
0
    def execute(self, userdata):
        global g_cmd_vel_pub
        if rospy.is_shutdown():
            return 'end'
        self.bumper_sub = rospy.Subscriber('mobile_base/events/bumper',
                                           BumperEvent, self.bump_callback)
        self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_callback)
        #rospy.wait_for_message('/Odom', Odometry)
        rospy.sleep(1)
        self.pushing = False
        self.stop = False

        ori_x = None
        ori_y = None
        while not self.stop:
            if ori_x == None and ori_y == None and self.odom[
                    'x'] != None and self.odom['y'] != None:
                ori_x = self.odom['x']
                ori_y = self.odom['y']
            twist = Twist()
            twist.linear.x = 0.1
            g_cmd_vel_pub.publish(twist)
            if self.bumper == 1:
                if self.state == 1:
                    self.pushing = True
                if self.state == 0:
                    self.pushing = False
            if self.bumper == 0 and self.state == 1:
                g_cmd_vel_pub.publish(Twist())
                util.rotate(5)
            if self.bumper == 2 and self.state == 1:
                g_cmd_vel_pub.publish(Twist())
                util.rotate(-5)
            print(ori_x, ori_y, self.odom['x'], self.odom['y'])
            if ori_x != None and ori_y != None:
                if abs(ori_x - self.odom['x']) > abs(2) or abs(
                        ori_y - self.odom['y']) > abs(2):
                    return 'completed'
Example #32
0
 def make_frame_nodes(self):
     allowed_x = [i*self.length_long for i in range(self.num_cols+1)]
     allowed_y = [i*self.length_across for i in range(self.num_rows+1)]
     name = 301
     for x in allowed_x:
         for y in allowed_y:
             x_rot, y_rot = rotate(x, y, self.course)
             self.nodes[name] = {
                 "pos": (x_rot, y_rot, -self.frame_depth),
                 "id": self.node_id,
                 "dof": True
             }
             name += 1
             self.node_id += 1
Example #33
0
 def points(self, shrink):
     points = []
     min_y = sys.maxint
     index = -1
     self.edge_list.reverse()
     for i, e in enumerate(self.edge_list):
         if e["y1"] < min_y:
             min_y = e["y1"]
             index = i
     self.edge_list = rotate(self.edge_list, index)
     for e in self.edge_list:
         (t_x, t_y) = interp(self.mid_x, self.mid_y, e["x1"], e["y1"], shrink)
         points.append({"x": t_x, "y": t_y})
     return points
Example #34
0
    def find_empty_cell(self, direction, rot):
        """ checks if self.pos + (direction +- rot) is an empty cell.
        If only one is empty, return the direction and position for it
        If both are empty, decide randomly which one to use.
        If none is empty, return None

        @return
        A (direction, position) tuple for the resulting cell found or (Direction.NONE, None),
        if all concerned cells are occupied.
        """

        # if direction == Direction.NONE:
        #     # if we don't have
        #     return Direction.NONE, None

        pdir = rotate(direction, +rot)
        mdir = rotate(direction, -rot)

        ppos = Grid.neighbour_pos_in_direction(self.pos, pdir)
        mpos = Grid.neighbour_pos_in_direction(self.pos, mdir)

        pempty = self.grid.is_empty(ppos)
        mempty = self.grid.is_empty(mpos)

        pres = (pdir, ppos)
        mres = (mdir, mpos)

        if pempty and mempty:
            # both are free, decide randomly
            return random.choice([pres, mres])
        elif pempty:
            return pres
        elif mempty:
            return mres
        else:
            # none is empty
            return Direction.NONE, None
Example #35
0
    def update_all(self, col):
        rgb = util.HSVtoRGB(*col)

        for spinbox, val in zip(self.hsv_controls + self.rgb_controls, [*col, *rgb]):
            spinbox.set(round(val, 2))
        self.create_line(rgb)

        self.canvas.delete(self.cursors[0])
        self.canvas.delete(self.cursors[1])
        self.move_cursor(0, *(util.rotate([col[1]*self.size/2,0], col[0]*2*math.pi) + self.size/2))
        self.move_cursor(1, self.size+10, (1-col[2])*(self.size-20)+10)

        self.refresh(col[2])

        self.colour = list(col)
Example #36
0
 def interpolate_force(self,q,ids):
   force = np.array([0.,0.,0.])
   sum = 0.
   for idd in ids:
     id = idd[0]
     [d,R] = self.Db.distance(self.Db.Neighborhoods[id],q) # recompute
     w = interpolate.rbf(d)
     f = np.array(self.Db.Forces[id]) 
     Rf = util.rotate(R,f) 
     force += w*np.array(Rf)
     #print f, Rf, w, force
     print w, Rf
     sum += w
   force /= sum
   return force 
Example #37
0
 def points(self, shrink):
     points = []
     min_y = sys.maxint
     index = -1
     self.edge_list.reverse()
     for i, e in enumerate(self.edge_list):
         if e["y1"] < min_y:
             min_y = e["y1"]
             index = i
     self.edge_list = rotate(self.edge_list, index)
     for e in self.edge_list:
         (t_x, t_y) = interp(self.mid_x, self.mid_y, e["x1"], e["y1"],
                             shrink)
         points.append({"x": t_x, "y": t_y})
     return points
Example #38
0
 def force_interpolation(self,Q,ids):
   n = len(ids)
   D = np.zeros((n,n))
   for i in range(len(ids)):
     for j in range(i+1,len(ids)):
       idx = self.index(ids[i],ids[j])
       D[i,j] = D[j,i] = self.Pair_distances[idx] 
   fs = []
   ds = []
   for i in ids:
     [d,R] = self.distance(self.Neighborhoods[i],Q)
     ds.append(d)
     fi = self.Forces[i]
     Rfi = util.rotate(R,fi)
     fs.append(Rfi)
   w = self.rbf.solve(D,fs)
   return self.rbf.interpolate(ds)
 def set_timeperiods(self):
     """sets optimization periods based on selection of optimization hours
     in the dispatch configuration
     sets:
       period_hours = range from 1 to the number of opt_hours
       periods = range from 1 to the maximum number of periods (i.e. 8760/period_hours)
       period_timepoints = dictionary with keys of period and values of period hours
       period_flex_load_timepoints = dictionary with keys of period and values of a nested dictionary with the keys of period_hours and the values of those period hours offset
       by the flexible_load_constraint_offset configuration parameter
     """
     if hasattr(self,'hours'):
         return
     self.num_hours = len(shape.shapes.active_dates_index)
     self.hours = range(self.num_hours)
     num_periods = int(round(self.num_hours / float(cfg.opt_period_length)))
     self.periods = range(num_periods)
     split_hours = [list(a) for a in np.array_split(self.hours, num_periods)] # splits into roughly equal lengths
     self.period_lengths = dict(zip(self.periods, [len(a) for a in split_hours]))
     self.period_timepoints = dict(zip(self.periods, split_hours))
     self.period_previous_timepoints = dict(zip(self.periods, [dict(zip(*(a, util.rotate(a,1)))) for a in split_hours]))
     self.period_repeated = util.flatten_list([[p]*self.period_lengths[p] for p in self.periods])
Example #40
0
 def rot(pair, rotation):
     im, angle = pair
     return rotate(im, rotation), wrap_angle(angle - rotation)
Example #41
0
 def update_angle(self, angle):
     self.angle = angle
     self.image = util.rotate(Steamboat.image, angle)
     self.rect.width = self.image.get_width()
     self.rect.height = self.image.get_height()
Example #42
0
if  __name__ == "__main__":
  print "!!! TEST !!!" 
  search = metric_search()
  c1 = [[1.1, 1, 1],
        [1, -1, -1],
        [-1, 1, -1],
        [-1, -1, 1]]
  C1 = np.array(c1)
  f = [1,2,3]
  F = np.array(f)
  theta = math.pi/3.
  c = math.cos(theta)
  s = math.sin(theta)
  R = [[c,-s,0],
       [s, c,0],
       [0, 0,1]]
  print "R=\n",np.array(R)
  RC1 = np.transpose(np.dot(R,np.transpose(C1)))
  print "A=  C1 = \n",C1
  print "B=R.C1 = \n",RC1
  RF  = np.dot(R,F)
  print "F @A=  F  = \n",F
  print "F @B=R.F  = \n",RF
  [d,OptRot] = search.Db.distance(C1,RC1)
  print " distance = ",d
  print " Optimal rotation =\n",OptRot
  OptRotTF = util.rotate(np.transpose(OptRot),F)
  print " Force at A rotated to B=\n",OptRotTF,"\n error =",util.error(RF,OptRotTF)
  OptRotRF = util.rotate(OptRot,RF)
  print " Force at B rotated to A=\n",OptRotRF,"\n error =",util.error(F,OptRotRF)
Example #43
0
def parse_trace(fname):
    ''' Parse McStas trace output from stdin and write results
        to file objects csv_comps and csv_lines '''

    color = 0

    # map from component name to (position, rotation matrix)
    comps = {}

    # active (position, rotation matrix)
    comp = (np.array([0, 0, 0]),
            np.array([1, 0, 0,
                      0, 1, 0,
                      0, 0, 1]).reshape(3,3))

    # previous neutron position
    prev = None
    skip = False
    # we are drawing a neutron
    active = False
    xstate=[]
    ystate=[]
    zstate=[]
    
    circlePoints = vtk.vtkPoints()
    circleLines = vtk.vtkCellArray()
    circle_pid = 0
    
    multiPoints = vtk.vtkPoints()
    multiLines = vtk.vtkCellArray()
    multi_pid = 0
    
    neutronPoints = vtk.vtkPoints()
    neutronLines = vtk.vtkCellArray()
    neutron_pid = 0
    
    f = open(fname, 'r')
    lines = f.readlines()
    
    for i, line in enumerate(lines):
        if not line:
            break
        line = line.strip()
        # register components
        if line.startswith(UC_COMP):
            # grab info line
            info = lines[i+1]
            assert info[:4] == 'POS:'
            nums = [x.strip() for x in info[4:].split(',')]
            # extract fields
            name = line[len(UC_COMP):].strip(' "\n')
            pos = np.array([float(x) for x in nums[:3]])
            # read flat 3x3 rotation matrix
            rot = np.array([float(x) for x in nums[3:3+9]]).reshape(3, 3)
            comps[name] = (pos, rot)

        # switch perspective
        elif line.startswith(MC_COMP):
            color += 1
            comp = comps[line[len(MC_COMP) + 1:]]

        elif line.startswith(MC_COMP_SHORT):
            name = line[len(MC_COMP_SHORT) + 1:].strip('"')
            comp = comps[name]
            skip = True

        # process multiline
        elif line.startswith(MC_LINE):
            points = parse_multiline(line[len(MC_LINE):].strip('()'))
            points.pop(0)
            coords = rotate_points(points, comp)
            beg = multi_pid
            for p in coords:
                multiPoints.InsertNextPoint(p)
                multi_pid += 1
            end = multi_pid
            for idx in range(beg, end-1):
                vline = vtk.vtkLine()
                vline.GetPointIds().SetId(0,idx)
                vline.GetPointIds().SetId(1,idx +1)
                multiLines.InsertNextCell(vline)
                
        # process circle
        elif line.startswith(MC_CIRCLE):
            xyz = 'xyz'
            items = line[len(MC_CIRCLE):].strip('()').split(',')
            # plane
            pla = [xyz.find(a) for a in items[0].strip("''")]
            # center and radius
            pos = [float(x) for x in items[1:4]]
            rad = float(items[4])
            coords = draw_circle(pla, pos, rad, comp)
            beg = circle_pid
            for p in coords:
                circlePoints.InsertNextPoint(p)
                circle_pid += 1
            end = circle_pid
            for idx in range(beg, end-1):
                vline = vtk.vtkLine()
                vline.GetPointIds().SetId(0,idx)
                vline.GetPointIds().SetId(1,idx +1)
                circleLines.InsertNextCell(vline)
        
                
        # activate neutron when it enters
        elif line.startswith(MC_ENTER):
            prev = None
            skip = True
            active = True
            color = 0
            xstate=[]
            ystate=[]
            zstate=[]
        # deactivate neutron when it leaves
        elif line.startswith(MC_LEAVE):
            
            coords = np.column_stack([xstate, ystate, zstate])
            beg = neutron_pid
            for p in coords:
                neutronPoints.InsertNextPoint(p)
                neutron_pid += 1
            end = neutron_pid
            for idx in range(beg, end-1):
                vline = vtk.vtkLine()
                vline.GetPointIds().SetId(0,idx)
                vline.GetPointIds().SetId(1,idx +1)
                neutronLines.InsertNextCell(vline)
            active = False
            prev = None
            
        elif line.startswith(MC_ABSORB):
            pass

        # register state and scatter
        elif line.startswith(MC_STATE) or line.startswith(MC_SCATTER):
            
            if not active:
                continue

            if skip:
                skip = False
                continue
            
            xyz = [float(x) for x in line[line.find(':')+1:].split(',')[:3]]
            xyz = rotate(xyz, comp)
            if prev is not None:
                xstate.append(xyz[0])
                ystate.append(xyz[1])
                zstate.append(xyz[2])
            prev = xyz
            xstate.append(prev[0])
            ystate.append(prev[1])
            zstate.append(prev[2])

    f.close()

    circlePolydata =vtk.vtkPolyData()
    circlePolydata.SetPoints(circlePoints)
    circlePolydata.SetLines(circleLines)

    circle_mapper = vtk.vtkPolyDataMapper()
    try:
      circle_mapper.SetInputData(circlePolydata) # VTK Python >= 6
    except:
      circle_mapper.SetInput(circlePolydata)     # VTK Python >= 5.8
    circle_actor = vtk.vtkActor()
    circle_actor.SetMapper(circle_mapper)
    circle_actor.GetProperty().SetAmbient(0.2)
    circle_actor.GetProperty().SetDiffuse(0.5)
    circle_actor.GetProperty().SetSpecular(0.3)
    circle_actor.GetProperty().SetColor(0,0.7,0.7)
    circle_actor.GetProperty().SetLineWidth(3)   
    
    
    multiPolydata =vtk.vtkPolyData()
    multiPolydata.SetPoints(multiPoints)
    multiPolydata.SetLines(multiLines)

    multi_mapper = vtk.vtkPolyDataMapper()
    try:
      multi_mapper.SetInputData(multiPolydata)
    except:
      multi_mapper.SetInput(multiPolydata)
    multi_actor = vtk.vtkActor()
    multi_actor.SetMapper(multi_mapper)
    multi_actor.GetProperty().SetAmbient(0.2)
    multi_actor.GetProperty().SetDiffuse(0.5)
    multi_actor.GetProperty().SetSpecular(0.3)
    multi_actor.GetProperty().SetColor(1,0,0.5)
    multi_actor.GetProperty().SetLineWidth(3)
    
    neutronPolydata =vtk.vtkPolyData()
    neutronPolydata.SetPoints(neutronPoints)
    neutronPolydata.SetLines(neutronLines)

    neutron_mapper = vtk.vtkPolyDataMapper()
    try:
      neutron_mapper.SetInputData(neutronPolydata)
    except:
      neutron_mapper.SetInput(neutronPolydata)
    neutron_actor = vtk.vtkActor()
    neutron_actor.SetMapper(neutron_mapper)
    neutron_actor.GetProperty().SetAmbient(0.2)
    neutron_actor.GetProperty().SetDiffuse(0.5)
    neutron_actor.GetProperty().SetSpecular(0.3)
    neutron_actor.GetProperty().SetColor(1,1,1)
    neutron_actor.GetProperty().SetLineWidth(2) 
    
    renderer = vtk.vtkRenderer()
    renderer.AddActor(circle_actor)
    renderer.AddActor(multi_actor)
    renderer.AddActor(neutron_actor)
    renderer.SetBackground(0, 0, 0)
    
    renwin = vtk.vtkRenderWindow()
    renwin.AddRenderer(renderer)
    
    iren = vtk.vtkRenderWindowInteractor()
    istyle = vtk.vtkInteractorStyleTrackballCamera() 
    iren.SetInteractorStyle(istyle)
    iren.SetRenderWindow(renwin)

    iren.Initialize()
    renwin.Render()
    iren.Start()    
Example #44
0
def parse_trace(world, fp=sys.stdin, inspectComp=None):
    """ Prase McStas trace output from stdin and write result to output """

    color = 0

    # def out_point((p_x, p_y, p_z)):
    #     ''' Write a line to csv_lines '''
    #     csv_lines.write('%s, %s, %s, %s\n' % (p_x, p_y, p_z, color))

    # print headers
    # csv_comps.write('name, x, y, z\n')
    # csv_lines.write('x, y, z, c\n')

    # map from component name to (position, rotation matrix)
    comps = {}

    # active (position, rotation matrix)
    comp = (np.array([0, 0, 0]), np.array([1, 0, 0, 0, 1, 0, 0, 0, 1]).reshape(3, 3))
    compName = ""

    # we are following a neutron
    active = False
    # we need to draw the neutron (it passed the "check-point"/inspect component)
    inspect = False
    # list of observed neutron positions
    neutron = []
    # skip next neutron position
    skip = False
    # total count of drawed neutrons
    neutrons_drawed = 0

    while True:
        # read line
        line = get_line(fp)
        if line is None:
            break

        # register components
        if line.startswith(UC_COMP):
            # grab info line
            info = get_line(fp)
            assert info[:4] == "POS:"
            nums = [x.strip() for x in info[4:].split(",")]
            # extract fields
            name = line[len(UC_COMP) :].strip(' "\n')
            pos = np.array([float(x) for x in nums[:3]])
            # read flat 3x3 rotation matrix
            rot = np.array([float(x) for x in nums[3 : 3 + 9]]).reshape(3, 3)
            comps[name] = (pos, rot)
            # csv_comps.write('%s, %s, %s, %s\n' % ((name,) + tuple(pos)))

        # switch perspective
        elif line.startswith(MC_COMP):
            color += 1
            name = line[len(MC_COMP) + 1 :].strip()
            compName = name
            comp = comps[name]

        elif line.startswith(MC_COMP_SHORT):
            name = line[len(MC_COMP_SHORT) + 1 :].strip('"')
            compName = name
            comp = comps[name]
            skip = True

        # process multiline
        elif line.startswith(MC_LINE):
            points = parse_multiline(line[len(MC_LINE) :].strip("()"))
            world.drawLine((rotate(p, comp) for p in points), color=getColor(color))

        # process circle
        elif line.startswith(MC_CIRCLE):
            xyz = "xyz"
            items = line[len(MC_CIRCLE) :].strip("()").split(",")
            # plane
            pla = [xyz.find(a) for a in items[0].strip("''")]
            # center and radius
            pos = [float(x) for x in items[1:4]]
            rad = float(items[4])
            points = draw_circle(pla, pos, rad, comp)
            world.drawLine(points, color=getColor(color))

        # activate neutron when it enters
        elif line.startswith(MC_ENTER):
            neutron = []
            skip = True
            active = True
            inspect = False
            color += 1

        # deactivate neutron when it leaves
        elif line.startswith(MC_LEAVE) or line.startswith(MC_ABSORB):
            active = False
            if inspectComp is None or inspect:
                world.drawLine(neutron, color="1 0 0")
                neutrons_drawed += 1

        # register state and scatter
        elif line.startswith(MC_STATE) or line.startswith(MC_SCATTER):

            if not active:
                continue

            if skip:
                skip = False
                continue

            if inspectComp and inspectComp == compName:
                # We will draw this neutron!
                inspect = True

            # keep track of points the neutron passes through
            xyz = [float(x) for x in line[line.find(":") + 1 :].split(",")[:3]]
            xyz = rotate(xyz, comp)
            neutron.append(xyz)

    print "Neutrons drawed:", neutrons_drawed, (inspectComp and "(reaching %s)" % inspectComp or "(all)")
    return world
Example #45
0
def parse_trace(csv_comps, csv_lines):
    ''' Prase McStas trace output from stdin and write results
        to file objects csv_comps and csv_lines '''

    color = 0
    def out_point((p_x, p_y, p_z)):
        ''' Write a line to csv_lines '''
        csv_lines.write('%s, %s, %s, %s\n' % (p_x, p_y, p_z, color))

    # print headers
    csv_comps.write('name, x, y, z\n')
    csv_lines.write('x, y, z, c\n')

    # map from component name to (position, rotation matrix)
    comps = {}

    # active (position, rotation matrix)
    comp = (np.array([0, 0, 0]),
            np.array([1, 0, 0,
                      0, 1, 0,
                      0, 0, 1]).reshape(3,3))

    # previous neutron position
    prev = None
    skip = False
    # we are drawing a neutron
    active = False

    while True:
        # read line
        line = get_line()
        if not line:
            break

        # register components
        if line.startswith(UC_COMP):
            # grab info line
            info = get_line()
            assert info[:4] == 'POS:'
            nums = [x.strip() for x in info[4:].split(',')]
            # extract fields
            name = line[len(UC_COMP):].strip(' "\n')
            pos = np.array([float(x) for x in nums[:3]])
            # read flat 3x3 rotation matrix
            rot = np.array([float(x) for x in nums[3:3+9]]).reshape(3, 3)
            comps[name] = (pos, rot)
            csv_comps.write('%s, %s, %s, %s\n' % ((name,) + tuple(pos)))

        # switch perspective
        elif line.startswith(MC_COMP):
            color += 1
            comp = comps[line[len(MC_COMP) + 1:]]

        elif line.startswith(MC_COMP_SHORT):
            name = line[len(MC_COMP_SHORT) + 1:].strip('"')
            comp = comps[name]
            skip = True

        # process multiline
        elif line.startswith(MC_LINE):
            points = parse_multiline(line[len(MC_LINE):].strip('()'))
            start = points.pop(0)
            while points:
                end = points.pop(0)
                for point in (start, end):
                    out_point(rotate(point, comp))
                start = end

        # process circle
        elif line.startswith(MC_CIRCLE):
            xyz = 'xyz'
            items = line[len(MC_CIRCLE):].strip('()').split(',')
            # plane
            pla = [xyz.find(a) for a in items[0].strip("''")]
            # center and radius
            pos = [float(x) for x in items[1:4]]
            rad = float(items[4])
            draw_circle(pla, pos, rad, comp, out_point)

        # activate neutron when it enters
        elif line.startswith(MC_ENTER):
            prev = None
            skip = True
            active = True
            color += 1

        # deactivate neutron when it leaves
        elif line.startswith(MC_LEAVE):
            active = False

        elif line.startswith(MC_ABSORB):
            pass

        # register state and scatter
        elif line.startswith(MC_STATE) or line.startswith(MC_SCATTER):

            if not active:
                continue

            if skip:
                skip = False
                continue

            xyz = [float(x) for x in line[line.find(':')+1:].split(',')[:3]]
            xyz = rotate(xyz, comp)
            if prev is not None:
                out_point(prev)
                out_point(xyz)

            prev = xyz
Example #46
0
 def update_angle(self, angle):
     self.angle = angle
     self.image = util.rotate(Cannonball.spec_image, angle)
     self.rect.width = self.image.get_width()
     self.rect.height = self.image.get_height()
Example #47
0
def parse_trace():
    ''' Parse McStas trace output from stdin and write results
        to file objects csv_comps and csv_lines '''

    mpl.rcParams['legend.fontsize'] = 10

    fig = plt.figure(figsize=plt.figaspect(0.5)*1.5)
    ax = fig.gca(projection='3d')
    ax.set_xlabel("z")
    ax.set_ylabel("x")
    ax.set_zlabel("y")
    ax.set_aspect('equal')
    #    ax.autoscale_view(scalex=False, scaley=False, scalez=False)
    color = 0

    # map from component name to (position, rotation matrix)
    comps = {}

    # active (position, rotation matrix)
    comp = (np.array([0, 0, 0]),
            np.array([1, 0, 0,
                      0, 1, 0,
                      0, 0, 1]).reshape(3,3))

    # previous neutron position
    prev = None
    skip = False
    # we are drawing a neutron
    active = False
    xstate=[]
    ystate=[]
    zstate=[]
    
    while True:
        # read line
        line = get_line()
        if not line:
            break

        # register components
        if line.startswith(UC_COMP):
            # grab info line
            info = get_line()
            assert info[:4] == 'POS:'
            nums = [x.strip() for x in info[4:].split(',')]
            # extract fields
            name = line[len(UC_COMP):].strip(' "\n')
            pos = np.array([float(x) for x in nums[:3]])
            # read flat 3x3 rotation matrix
            rot = np.array([float(x) for x in nums[3:3+9]]).reshape(3, 3)
            comps[name] = (pos, rot)

        # switch perspective
        elif line.startswith(MC_COMP):
            color += 1
            comp = comps[line[len(MC_COMP) + 1:]]

        elif line.startswith(MC_COMP_SHORT):
            name = line[len(MC_COMP_SHORT) + 1:].strip('"')
            comp = comps[name]
            skip = True

        # process multiline
        elif line.startswith(MC_LINE):
            points = parse_multiline(line[len(MC_LINE):].strip('()'))
            start = points.pop(0)
            (x, y, z) = rotate_points(points, comp)
            ax.plot(z, x, y)

        # process circle
        elif line.startswith(MC_CIRCLE):
            xyz = 'xyz'
            items = line[len(MC_CIRCLE):].strip('()').split(',')
            # plane
            pla = [xyz.find(a) for a in items[0].strip("''")]
            # center and radius
            pos = [float(x) for x in items[1:4]]
            rad = float(items[4])
            (x,y,z) = draw_circle(pla, pos, rad, comp)
            ax.plot(z, x, y)
            
        # activate neutron when it enters
        elif line.startswith(MC_ENTER):
            prev = None
            skip = True
            active = True
            color = 0
            xstate=[]
            ystate=[]
            zstate=[]

        # deactivate neutron when it leaves
        elif line.startswith(MC_LEAVE):
            ax.plot(zstate, xstate, ystate)
            active = False
            prev = None
            
        elif line.startswith(MC_ABSORB):
            pass

        # register state and scatter
        elif line.startswith(MC_STATE) or line.startswith(MC_SCATTER):
            
            if not active:
                continue

            if skip:
                skip = False
                continue
            
            xyz = [float(x) for x in line[line.find(':')+1:].split(',')[:3]]
            xyz = rotate(xyz, comp)
            if prev is not None:
                xstate.append(xyz[0])
                ystate.append(xyz[1])
                zstate.append(xyz[2])
            prev = xyz
            xstate.append(prev[0])
            ystate.append(prev[1])
            zstate.append(prev[2])

        # kick out legacy "junk"
        elif line.startswith(MC_MAGNIFY) or line.startswith(MC_START) or line.startswith(MC_END) or line.startswith(MC_STOP):
            continue
        else:
            print line


    # A little bit of logic for controlling the aspect ratios/view
    (xmin, xmax)=ax.get_xlim3d()
    (ymin, ymax)=ax.get_ylim3d()
    (zmin, zmax)=ax.get_zlim3d()
    dx = xmax - xmin
    dy = ymax - ymin
    dz = zmax - zmin
    dmax=max(dx,dy,dz)

    # Check ranges and define axis box of max length cubed
    if dmax > dx:
        mean=(xmax+xmin)/2
        xmin=mean-dmax/2
        xmax=mean+dmax/2
    if dmax > dy:
        mean=(ymax+ymin)/2
        ymin=mean-dmax/2
        ymax=mean+dmax/2
    if dmax > dz:
        mean=(zmax+zmin)/2
        zmin=mean-dmax/2
        zmax=mean+dmax/2
        
    # Set new axis limits
    ax.set_xlim3d(xmin,xmax)
    ax.set_ylim3d(ymin,ymax)
    ax.set_zlim3d(zmin,zmax)
 
    plt.show()
Example #48
0
File: CMT.py Project: Mapiarz/CMT
	def process_frame(self, im_gray):

		tracked_keypoints, status = util.track(self.im_prev, im_gray, self.active_keypoints)
		(center, scale_estimate, rotation_estimate, tracked_keypoints) = self.estimate(tracked_keypoints)

		#Detect keypoints, compute descriptors
		keypoints_cv = self.detector.detect(im_gray) 
		keypoints_cv, features = self.descriptor.compute(im_gray, keypoints_cv)

		#Create list of active keypoints
		active_keypoints = zeros((0,3)) 

		#For each keypoint and its descriptor
		if len(keypoints_cv) > 0:
			for (keypoint_cv, feature) in zip(keypoints_cv, features):

				#Retrieve keypoint location
				location = np.array(keypoint_cv.pt)

				#First: Match over whole image
				#Compute distances to all descriptors
				matches = self.matcher.match(self.features_database, feature[None,:])
				distances = np.array([m.distance for m in matches])

				#Convert distances to confidences, do not weight
				combined = 1 - distances / self.DESC_LENGTH

				classes = self.database_classes

				#Sort in descending order
				sorted_conf = argsort(combined)[::-1] #reverse

				#Get best and second best index
				bestInd = sorted_conf[0]
				secondBestInd = sorted_conf[1]

				#Compute distance ratio according to Lowe
				ratio = (1-combined[bestInd]) / (1-combined[secondBestInd])

				#Extract class of best match
				keypoint_class = classes[bestInd]

				#If distance ratio is ok and absolute distance is ok and keypoint class is not background
				if ratio < self.THR_RATIO and combined[bestInd] > self.THR_CONF and keypoint_class != 0:

					#Add keypoint to active keypoints
					new_kpt = append(location, keypoint_class)
					active_keypoints = append(active_keypoints, array([new_kpt]), axis=0)

				#In a second step, try to match difficult keypoints
				#If structural constraints are applicable
				if not any(isnan(center)):

					#Compute distances to initial descriptors
					matches = self.matcher.match(self.selected_features, feature[None,:])
					distances = np.array([m.distance for m in matches])

					#Convert distances to confidences
					confidences = 1 - distances / self.DESC_LENGTH

					#Compute the keypoint location relative to the object center
					relative_location = location - center

					#Compute the distances to all springs
					displacements = util.L2norm(scale_estimate * util.rotate(self.springs, -rotation_estimate) - relative_location)

					#For each spring, calculate weight
					weight = displacements < self.THR_OUTLIER #Could be smooth function

					combined = weight * confidences

					classes = self.selected_classes

					#Sort in descending order
					sorted_conf = argsort(combined)[::-1] #reverse

					#Get best and second best index
					bestInd = sorted_conf[0]
					secondBestInd = sorted_conf[1]

					#Compute distance ratio according to Lowe
					ratio = (1-combined[bestInd]) / (1-combined[secondBestInd])

					#Extract class of best match
					keypoint_class = classes[bestInd]

					#If distance ratio is ok and absolute distance is ok and keypoint class is not background
					if ratio < self.THR_RATIO and combined[bestInd] > self.THR_CONF and keypoint_class != 0:

						#Add keypoint to active keypoints
						new_kpt = append(location, keypoint_class)

						#Check whether same class already exists
						if active_keypoints.size > 0:
							same_class = np.nonzero(active_keypoints[:,2] == keypoint_class)
							active_keypoints = np.delete(active_keypoints, same_class, axis=0)

						active_keypoints = append(active_keypoints, array([new_kpt]), axis=0)

		#If some keypoints have been tracked
		if tracked_keypoints.size > 0:

			#Extract the keypoint classes
			tracked_classes = tracked_keypoints[:,2]

			#If there already are some active keypoints
			if active_keypoints.size > 0:

				#Add all tracked keypoints that have not been matched
				associated_classes = active_keypoints[:,2]
				missing = ~np.in1d(tracked_classes, associated_classes)
				active_keypoints = append(active_keypoints, tracked_keypoints[missing,:], axis=0)

			#Else use all tracked keypoints
			else:
				active_keypoints = tracked_keypoints

		#Update object state estimate
		active_keypoints_before = active_keypoints
		self.center = center
		self.scale_estimate = scale_estimate
		self.rotation_estimate = rotation_estimate
		self.tracked_keypoints = tracked_keypoints
		self.active_keypoints = active_keypoints
		self.im_prev = im_gray
		self.keypoints_cv = keypoints_cv
		toc = time.time()

		self.tl = (nan,nan)
		self.tr = (nan,nan)
		self.br = (nan,nan)
		self.bl = (nan,nan)

		self.bb = array([nan,nan,nan,nan])

		self.has_result = False
		if not any(isnan(self.center)) and self.active_keypoints.shape[0] > self.num_initial_keypoints / 10:
			self.has_result = True

			tl = util.array_to_int_tuple(center + scale_estimate*util.rotate(self.center_to_tl[None,:], rotation_estimate).squeeze())
			tr = util.array_to_int_tuple(center + scale_estimate*util.rotate(self.center_to_tr[None,:], rotation_estimate).squeeze())
			br = util.array_to_int_tuple(center + scale_estimate*util.rotate(self.center_to_br[None,:], rotation_estimate).squeeze())
			bl = util.array_to_int_tuple(center + scale_estimate*util.rotate(self.center_to_bl[None,:], rotation_estimate).squeeze())

			min_x = min((tl[0],tr[0],br[0],bl[0]))
			min_y = min((tl[1],tr[1],br[1],bl[1]))
			max_x = max((tl[0],tr[0],br[0],bl[0]))
			max_y = max((tl[1],tr[1],br[1],bl[1]))

			self.tl = tl
			self.tr = tr
			self.bl = bl
			self.br = br

			self.bb = np.array([min_x, min_y, max_x - min_x, max_y - min_y])
Example #49
0
File: CMT.py Project: Mapiarz/CMT
	def estimate(self, keypoints):

		center = array((nan,nan))
		scale_estimate = nan
		med_rot = nan

		#At least 2 keypoints are needed for scale
		if keypoints.size > 1:

			#Extract the keypoint classes
			keypoint_classes = keypoints[:,2].squeeze().astype(np.int) 

			#Retain singular dimension
			if keypoint_classes.size == 1:
				keypoint_classes = keypoint_classes[None]

			#Sort
			ind_sort = argsort(keypoint_classes)
			keypoints = keypoints[ind_sort]
			keypoint_classes = keypoint_classes[ind_sort]

			#Get all combinations of keypoints
			all_combs = array([val for val in itertools.product(range(keypoints.shape[0]),repeat = 2)])	

			#But exclude comparison with itself
			all_combs = all_combs[all_combs[:,0] != all_combs[:,1],:]

			#Measure distance between allcombs[0] and allcombs[1]
			ind1 = all_combs[:,0] 
			ind2 = all_combs[:,1]

			class_ind1 = keypoint_classes[ind1] - 1
			class_ind2 = keypoint_classes[ind2] - 1

			duplicate_classes = class_ind1 == class_ind2

			if not all(duplicate_classes):
				ind1 = ind1[~duplicate_classes]
				ind2 = ind2[~duplicate_classes]

				class_ind1 = class_ind1[~duplicate_classes]
				class_ind2 = class_ind2[~duplicate_classes]

				pts_allcombs0 = keypoints[ind1,:2]
				pts_allcombs1 = keypoints[ind2,:2]

				#This distance might be 0 for some combinations,
				#as it can happen that there is more than one keypoint at a single location
				dists = util.L2norm(pts_allcombs0 - pts_allcombs1)

				original_dists = self.squareform[class_ind1,class_ind2]

				scalechange = dists / original_dists

				#Compute angles
				angles = np.empty((pts_allcombs0.shape[0]))

				v=pts_allcombs1 - pts_allcombs0
				angles = np.arctan2(v[:,1],v[:,0])
				
				original_angles = self.angles[class_ind1,class_ind2]

				angle_diffs = angles - original_angles

				#Fix long way angles
				long_way_angles = np.abs(angle_diffs) > math.pi

				angle_diffs[long_way_angles] = angle_diffs[long_way_angles] - np.sign(angle_diffs[long_way_angles]) * 2 * math.pi

				scale_estimate = median(scalechange)
				if not self.estimate_scale:
					scale_estimate = 1;

				med_rot = median(angle_diffs)
				if not self.estimate_rotation:
					med_rot = 0;

				keypoint_class = keypoints[:,2].astype(np.int)
				votes = keypoints[:,:2] - scale_estimate * (util.rotate(self.springs[keypoint_class-1], med_rot))

				#Remember all votes including outliers
				self.votes = votes

				#Compute pairwise distance between votes
				pdist = scipy.spatial.distance.pdist(votes)

				#Compute linkage between pairwise distances
				linkage = scipy.cluster.hierarchy.linkage(pdist)

				#Perform hierarchical distance-based clustering
				T = scipy.cluster.hierarchy.fcluster(linkage, self.THR_OUTLIER, criterion='distance')

				#Count votes for each cluster
				cnt = np.bincount(T) #Dummy 0 label remains
				
				#Get largest class
				Cmax = argmax(cnt)

				#Identify inliers (=members of largest class)
				inliers = T == Cmax
				#inliers = med_dists < THR_OUTLIER

				#Remember outliers
				self.outliers = keypoints[~inliers,:]

				#Stop tracking outliers
				keypoints = keypoints[inliers,:]

				#Remove outlier votes
				votes = votes[inliers,:]

				#Compute object center
				center = np.mean(votes, axis=0)

		return (center, scale_estimate, med_rot, keypoints)