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)}
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'
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()
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))
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()
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)
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'])
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'
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)
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()
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)
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)
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
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
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)))
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)
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])
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))
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)))
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
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
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))
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)
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'
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()
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
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
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'
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
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
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
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)
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
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])
def rot(pair, rotation): im, angle = pair return rotate(im, rotation), wrap_angle(angle - rotation)
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()
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)
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()
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
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
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()
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()
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])
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)