def extract_car_position(self, distance, camera_angle, x, W): angle = (x - (W / 2)) / ( W / 2) * GeneralParameters.YOLO_HALF_CAMERA_APERTURE_ANGLE angle = math.radians(angle) adjacent_side = math.cos(angle) * distance opposite_side = math.sin(angle) * distance if self.debug: self.log.d(self.TAG, f'X= {x}, W= {W}, Distance= {distance}') self.log.d( self.TAG, f'angle= {angle}, camera_angle= {int(math.cos(math.radians(camera_angle)))}, ' f'adj= {adjacent_side}, ops= {opposite_side}') self.log.d( self.TAG, f'angle: cos= {math.cos(angle)}, sin= {math.sin(angle)}') if int(math.cos(math.radians(camera_angle))) != 0: pos_x = opposite_side * math.cos(math.radians(camera_angle)) pos_y = adjacent_side * math.cos(math.radians(camera_angle)) if self.debug: self.log.i(self.TAG, f'position: front/rear - X= {pos_x}, Y= {pos_y}') return Point(pos_x, pos_y) else: pos_y = opposite_side * int(math.sin(math.radians(camera_angle))) pos_x = adjacent_side * int(math.sin( math.radians(camera_angle))) * -1 if self.debug: self.log.i(self.TAG, f'position: side - X= {pos_x}, Y= {pos_y}') return Point(pos_x, pos_y)
def get_corners(self): x, y, w, h = self.rect cx, cy = x + w / 2, y + h / 2 return [ Point(cx - w / 2, cy - h / 2), Point(cx + w / 2, cy - h / 2), Point(cx + w / 2, cy + h / 2), Point(cx - w / 2, cy + h / 2) ]
def get_corners(self): cx, cy = self.get_actual_center().to_tuple() _, _, w, h = self.original_image.get_rect() simple_corners = [ Point(cx - w / 2, cy - h / 2), Point(cx + w / 2, cy - h / 2), Point(cx + w / 2, cy + h / 2), Point(cx - w / 2, cy + h / 2) ] return rotate(simple_corners, Point(cx, cy), -math.radians(self.angle))
class SnakeDirections(object): """ Defines all possible directions the snake can take, as well as the corresponding offsets. """ NORTH = Point(0, -1) EAST = Point(1, 0) SOUTH = Point(0, 1) WEST = Point(-1, 0)
def simulation(iterations): slam = EKF() # history #hxEst = slam.xEst for _ in range(int(iterations)): uDT = Point(0, 0, np.random.randn()) uDT.hypo = np.random.randn() x_state = slam.estimate(movement=uDT) # SEULE LIGNE IMPORTANTE x_state = [[x_state.x], [x_state.y], [x_state.theta]]
def spawn_9(self): parking_spot_index = randint(3, 8) for i in range(0, 9): if i == parking_spot_index: self.scene.add_park( ParkingSpot(Point(15 + i * 64 + i * 15 - 15, 250), normal_angle=270)) else: self.scene.add_obstacle( Obstacle( Point(15 + i * 64 + i * 15 + randint(-3, 3), 270 + randint(-3, 3)), obstacle_images[randint(0, 4)]))
def set_scenario(self, scenario): number_of_spots = np.random.choice(scenario.spawn_count) self.spawn_x(number_of_spots, scenario.lowest_spot) car_x_offset = 20 car_y_offset = 20 car_x_limit = scenario.random_car_x car_y_limit = scenario.random_car_y self.scene.set_car( Car((car_x_offset + int(car_x_limit * np.random.random_sample()), car_y_offset + int(car_y_limit * np.random.random_sample())))) self.scene.add_obstacle(Obstacle(Point(-2, 0), 'obstacle_width.png')) self.scene.add_obstacle(Obstacle(Point(720, 0), 'obstacle_width.png')) self.scene.add_obstacle(Obstacle(Point(0, -2), 'obstacle_length.png')) self.scene.add_obstacle(Obstacle(Point(0, 420), 'obstacle_length.png'))
def __init__(self, x, y, w, h): self.x = x self.y = y self.w = w self.h = h self.center_x = x + w / 2 self.center_y = y + h / 2 self.top_left_point = Point(x, y) self.bottom_right_point = Point(x + w, y + h) self.center_point = Point(self.center_x, self.center_y) self.area = w * h self.diameter_length = (self.top_left_point - self.bottom_right_point).len() self.aspect_ratio = self.w / self.h
class Snake: def __init__(self, grid_size): self.grid_size = grid_size self.direction_string = "right" self.directions = { "right": Point(+1, 0), "left": Point(-1, 0), "up": Point(0, -1), "down": Point(0, +1), } self.direction_step = self.directions[self.direction_string] self.head_position = Point(self.grid_size // 2, self.grid_size // 2) self.positions = OrderedSet([ # Tail at the start, head at the end self.head_position - self.direction_step, self.head_position.copy(), ]) self.length = len(self.positions) self.steps = 0 pass def move(self): self.head_position += self.direction_step self.steps += 1 self.positions.add(self.head_position.copy()) def clip_tail(self): self.positions.remove_first() def turn(self, direction_string): if direction_string: self.direction_string = direction_string self.direction_step = self.directions[self.direction_string] def is_valid_inside_grid(self): for pos in self.positions.data: if (pos.x < 0 or pos.y < 0 or pos.x >= self.grid_size or pos.y >= self.grid_size): logger.info("Snake exited game grid") return False return True def is_valid_crossed_itself(self): if self.positions.n_unique_items() != self.positions.n_current_items: logger.info("Snake crossed itself") logger.info(self.positions) return False return True
def read_data(self, tr, dr): for i in range(0, 182): path = './data/%03d/Trajectory/' % i files = os.listdir(path) user = User(i) # 创建User对象 file_num = 0 for file in files: file_num += 1 trajectory = Trajectory() # 创建Trajectory对象 file_name = os.path.join(path, file) # 定位路径并打开文件 with open(file_name) as f: # plt数据文件的前六行无用 line_num = 0 for line in f: line_num += 1 if line_num <= 6: continue content = line.split(',') lat, lng, date, time = float(content[0]), float(content[1]), content[5], content[6][0:-1] # 所有的初始数据都是string格式 timestamp = date + ' ' + time point = Point(lng, lat, timestamp) trajectory.add_point(point) trajectory.staypoint_detection(tr, dr) if trajectory.staypoints: # 如果这条轨迹中检测到停留点的话 user.add_trajectory(trajectory) print(i, 'th user,', file_num, 'th trajectory,', len(trajectory.staypoints), 'staypoints') user.trajectory2staypoint_hist()
def __init__(self, x=0, y=0, left_top=(0, 0), area=0, perimeter=0, points=[]): """ Initalizes all shape variables.\n Variables: (6) 1. x - inherited from Point class 2. y - inherited from Point class 3. left_top - (x, y) coordinates 4. area - area calculation of the shape 5. perimeter - perimeter calculation of the shape 6. points - tuple of point locations (up to 4 sets of points) """ Point.__init__(self, x, y) self._left_top = (x, y) self._area = area self._perimeter = perimeter self._points = list(points)
def make_wall_tile(self, tl, tr, bl, br): unit = (int)(Connected.res / 2) res = Texture2D(Point(Connected.res, Connected.res)) # tl for x in range(0, unit): for y in range(0, Connected.margin): res.set_pixel(x, y, tl.get_pixel(x, y)) # tr for x in range(unit, unit * 2): for y in range(0, Connected.margin): res.set_pixel(x, y, tr.get_pixel(x, y)) # bl for x in range(0, unit): for y in range( Connected.margin, Connected.res): # why 10 instead of Connected.margin ? res.set_pixel(x, y, bl.get_pixel(x, y)) # br for x in range(unit, unit * 2): for y in range(Connected.margin, Connected.res): res.set_pixel(x, y, br.get_pixel(x, y)) return res
def spawn_x(self, x, lowest_spot): parking_spot_index = randint(lowest_spot, x - 1) margin = 105 - 10 * x for i in range(0, x): if i == parking_spot_index: self.scene.add_park( ParkingSpot(Point(15 + i * 64 + i * margin - 15, 250), normal_angle=270, allow_reverse_parking=self.scenario. allow_reverse_parking)) else: self.scene.add_obstacle( Obstacle( Point(15 + i * 64 + i * margin + randint(-3, 3), 270 + randint(-3, 3)), obstacle_images[randint(0, 4)]))
def __init__(self, position=Point(0, 0), sprite_name=None): if sprite_name is not None: self.image = pygame.image.load('sprites/' + sprite_name).convert_alpha() self.rect = self.image.get_rect() self.name = "ok" self.position = position
def __load_RFID(RFID_input): RFID = [] if type(RFID_input[0]) == type(Point(0, 0, 0)): for rfid in RFID_input: RFID.append(rfid.x, rfid.y) elif type(RFID_input[0]) == type([]): RFID = RFID_input return np.array(RFID)
def find_snake_head(self): """ Find the snake's head on the field. """ for y in range(self.size): for x in range(self.size): if self[(x, y)] == CellType.SNAKE_HEAD: return Point(x, y) raise ValueError('Initial snake position ' 'not specified on the level map')
def get_base(self): for i in range(0, 4): self.texture_base.insert( i, Texture2D(Point(Connected.res, Connected.res))) for x in range(i * Connected.res, (i + 1) * Connected.res): for y in range(0, Connected.res): self.texture_base[i].set_pixel( x - (i * Connected.res), y, self.get_pixel_from_base(x, y))
def get_observation(self): closely = [] for corner in self.car.get_corners(): closely.append(corner - self.closest_obstacle(corner)) car_center = self.car.get_actual_center() to_goal = car_center - self.parking_spots[0].get_actual_center() if OBS_POLAR: closely, to_goal = convert_to_polar(closely), convert_to_polar( to_goal) new_arr = [] for close in closely: new_arr.append( Point( float(close.x) / INITIAL_DISTANCE_BENCH, float(close.y))) closely = new_arr to_goal = Point(to_goal.x / INITIAL_DISTANCE_BENCH, to_goal.y) else: new_arr = [] for close in closely: new_arr.append( Point( float(close.x) / self.size[0], float(close.y) / self.size[1])) closely = new_arr to_goal = Point(to_goal.x / self.size[0], to_goal.y / self.size[1]) return [ float(self.car.angle) / 360, float(self.car.speed) / 5, # float(car_center.x) / self.size[0], float(car_center.y) / self.size[1], closely[0].x, closely[0].y, closely[1].x, closely[1].y, closely[2].x, closely[2].y, closely[3].x, closely[3].y, to_goal.x, to_goal.y ]
def parse_cities(reader) -> dict: cities = {} for line_no, line in enumerate(reader): try: city, x, y = line.split() x = float(x) y = float(y) cities[city] = Point(x, y) except Exception as e: raise ParseException(line_no, str(e)) return cities
def simulation(iterations): show_animation = True slam = EKF() # history hxEst = slam.xEst hxTrue = slam.xTrue for _ in range(int(iterations)): uDT = Point(0, 0, np.random.randn()) uDT.hypo = np.random.randn() x_state = slam.estimate(movement=uDT, RFID=[], simulation=True) # SEULE LIGNE IMPORTANTE x_state = [[x_state.x], [x_state.y], [x_state.theta]] # Affichage graphique if show_animation: import matplotlib.pyplot as plt # store data history hxEst = np.hstack((hxEst, x_state)) hxTrue = np.hstack((hxTrue, slam.xTrue)) if show_animation: # pragma: no cover plt.cla() plt.plot(slam.RFID[:, 0], slam.RFID[:, 1], "*k") plt.plot(slam.xEst[0], slam.xEst[1], ".r") # plot landmark n_LM = int((len(slam.xEst) - 3.0) / 2.0) for i in range(n_LM): plt.plot(slam.xEst[3 + i * 2], slam.xEst[3 + i * 2 + 1], "xg") plt.plot(hxTrue[0, :], hxTrue[1, :], "-b") plt.plot(hxEst[0, :], hxEst[1, :], "-r") plt.axis("equal") plt.grid(True) plt.pause(0.001)
def random_vertical_scenario(self): number_of_spots = randint(7, 9) if number_of_spots == 7: self.spawn_7() elif number_of_spots == 8: self.spawn_8() else: self.spawn_9() car_x_offset = 20 car_y_offset = 30 car_x_limit = 570 car_y_limit = 180 self.scene.set_car( Car((car_x_offset + int(car_x_limit * np.random.random_sample()), car_y_offset + int(car_y_limit * np.random.random_sample())))) # self.scene.set_car(Car((car_x_offset + car_x_limit, car_y_offset + car_y_limit))) self.scene.add_obstacle(Obstacle(Point(-2, 0), 'obstacle_width.png')) self.scene.add_obstacle(Obstacle(Point(720, 0), 'obstacle_width.png')) self.scene.add_obstacle(Obstacle(Point(0, -2), 'obstacle_length.png')) self.scene.add_obstacle(Obstacle(Point(0, 420), 'obstacle_length.png'))
def __init__(self, head_coordinates: Point, length: int = 3): """ Create a new snake. Args: head_coordinates: initial position of the snake. length: initial length of the snake. """ # Place the snake vertically, heading north. self.body = deque([ Point(head_coordinates.x, head_coordinates.y + i) for i in range(length) ]) self.direction = SnakeDirections.NORTH
def get_cameras_images(self): if self.debug: self.log.i(self.TAG, f'Getting images') layer_names = self.yolo_network.getLayerNames() layer_names = [ layer_names[i[0] - 1] for i in self.yolo_network.getUnconnectedOutLayers() ] cars_list: List[ImgCar] = list() for camera in self.cameras: image = camera.try_to_capture_image() (H, W) = image.shape[:2] blob = cv2.dnn.blobFromImage(image, 1 / 255.0, (416, 416), swapRB=True, crop=False) self.yolo_network.setInput(blob) layer_outputs = self.yolo_network.forward(layer_names) for output in layer_outputs: for detection in output: scores = detection[5:] if np.argmax(scores) in self.LABELS_TO_DETECT and scores[ np.argmax( scores)] > GeneralParameters.YOLO_CONFIDENCE: (x, y, w, h) = detection[0:4] * np.array([W, H, W, H]) distance = self.extract_distance_car_and_camera(H, h) if distance > GeneralParameters.YOLO_MAX_DISTANCE: continue class_id = int(np.argmax(scores)) score = scores[class_id] b_box = BoundingBox( Point(x, y), Size(w, h), self.extract_b_box_as_image(image, x, y, w, h), score) position = self.extract_car_position( distance, camera.angle, x, W) img_car = ImgCar(b_box, position) if self.debug: self.log.d(self.TAG, img_car) cars_list.append(img_car) self.frame = Frame(cars_list) return self.frame.cars
def create_level(self): """ Create a new field based on the level map. """ try: self._cells = np.array( [[self._level_map_to_cell_type[symbol] for symbol in line] for line in self.level_map]) except KeyError as err: raise ValueError(f'Unknown level map symbol: "{err.args[0]}"') assert self._cells.dtype != np.object, 'Invalid format. Level map ' \ 'must be a square' self._empty_cells = { Point(x, y) for y in range(self.size) for x in range(self.size) if self[(x, y)] == CellType.EMPTY }
def estimate(self, movement): ''' movement : (distance, angle) effectué ''' self.RFID = np.array( Lidar.get_RFID()) #get_RFID List([Dist, Angle]) en relatif movement = (movement[0] * 0.001, EKF.__pi_2_pi(movement[1] * math.pi / 180)) uDT = np.array([[ movement[0], movement[1] ]]).T # Movement in m and rad (only parameter to update) z, ud = self.__observation(uDT, RFID) self.xEst, self.__PEst = self.__ekf_slam(self.xEst, self.__PEst, ud, z) res = Point(self.xEst[0, 0] * 1000, self.xEst[1, 0] * 1000, self.xEst[2, 0] * 180 / math.pi) return res #(x,y,yaw) estimation of the robot
def estimate(self, movement, RFID=[], simulation=False): ''' movement : Point RFID : Liste de Point ''' if len(RFID) == 0: RFID = self.RFID if type(RFID) != type(np.array(0)): RFID = EKF.__load_RFID(RFID) if movement.hypo == 0: movement.set_parcour() uDT = np.array([[ movement.hypo, movement.theta ]]).T # Movement in m and rad (only parameter to update) self.xTrue, z, ud = self.__observation(self.xTrue, uDT, RFID, simulation) self.xEst, self.__PEst = self.__ekf_slam(self.xEst, self.__PEst, ud, z) #x_state = self.xEst[0:self.__STATE_SIZE] #(x,y,yaw) estimation of the robot return Point(self.xEst[0, 0], self.xEst[1, 0], self.xEst[2, 0])
def render_edges(self): L = 157 l = self.size+2 for (tail, head), count in self.edge_counts.items(): if tail is not head: h, t = Point(*self.nodes[head]), Point(*self.nodes[tail]) s = (h-t).norm() h,t = t+(h-t)*l/s, h-(h-t)*l/s theta = (h-t).ang() d = L/(count+1) pl = (h+t)/2 + Point(cos(theta), -sin(theta))*(L/2) ps = [pl + Point(-cos(theta),sin(theta))*d*(k+1) for k in range(count)] for p in ps: self.create_line(*h.crd(), *p.crd(), *t.crd(), fill=self.color['edges'], width=1, smooth=True, arrow=LAST, arrowshape=(8,9,4))
def __init__(self, grid_size): self.grid_size = grid_size self.direction_string = "right" self.directions = { "right": Point(+1, 0), "left": Point(-1, 0), "up": Point(0, -1), "down": Point(0, +1), } self.direction_step = self.directions[self.direction_string] self.head_position = Point(self.grid_size // 2, self.grid_size // 2) self.positions = OrderedSet([ # Tail at the start, head at the end self.head_position - self.direction_step, self.head_position.copy(), ]) self.length = len(self.positions) self.steps = 0 pass
def pseudo_random_vertical_scenario(self): self.scene.add_park(ParkingSpot(Point(370, 270))) car_x_offset = 20 car_y_offset = 20 car_x_limit = 570 car_y_limit = 180 self.scene.set_car( Car((car_x_offset + int(car_x_limit * np.random.random_sample()), car_y_offset + int(car_y_limit * np.random.random_sample())))) self.scene.add_obstacle( Obstacle(Point(20 + randint(-3, 3), 280 + randint(-3, 3)), obstacle_images[randint(0, 2)])) self.scene.add_obstacle( Obstacle(Point(110 + randint(-3, 3), 280 + randint(-3, 3)), obstacle_images[randint(0, 2)])) self.scene.add_obstacle( Obstacle(Point(200 + randint(-3, 3), 280 + randint(-3, 3)), obstacle_images[randint(0, 2)])) self.scene.add_obstacle( Obstacle(Point(290 + randint(-3, 3), 280 + randint(-3, 3)), obstacle_images[randint(0, 2)])) self.scene.add_obstacle( Obstacle(Point(470 + randint(-3, 3), 280 + randint(-3, 3)), obstacle_images[randint(0, 2)])) self.scene.add_obstacle( Obstacle(Point(560 + randint(-3, 3), 280 + randint(-3, 3)), obstacle_images[randint(0, 2)])) self.scene.add_obstacle( Obstacle(Point(640 + randint(-3, 3), 280 + randint(-3, 3)), obstacle_images[randint(0, 2)])) self.scene.add_obstacle(Obstacle(Point(-2, 0), 'obstacle_width.png')) self.scene.add_obstacle(Obstacle(Point(720, 0), 'obstacle_width.png')) self.scene.add_obstacle(Obstacle(Point(0, -2), 'obstacle_length.png')) self.scene.add_obstacle(Obstacle(Point(0, 420), 'obstacle_length.png'))
def gen(self): self.result = [] #: 0 self.result.insert(0, self.texture_base[0]) #: 1 tex = Texture2D(Point(Connected.res, Connected.res)) tex = self.make_wall_tile(self.texture_base[2], self.texture_base[2], self.texture_base[0], self.texture_base[0]) self.result.insert(1, tex) #: 2 tex = Texture2D(Point(Connected.res, Connected.res)) tex = self.make_wall_tile(self.texture_base[1], self.texture_base[0], self.texture_base[1], self.texture_base[0]) self.result.insert(2, tex) #: 3 tex = Texture2D(Point(Connected.res, Connected.res)) tex = self.make_wall_tile(self.texture_base[3], self.texture_base[2], self.texture_base[1], self.texture_base[0]) self.result.insert(3, tex) #: 4 tex = Texture2D(Point(Connected.res, Connected.res)) tex = self.make_wall_tile(self.texture_base[0], self.texture_base[1], self.texture_base[0], self.texture_base[1]) self.result.insert(4, tex) #: 5 tex = Texture2D(Point(Connected.res, Connected.res)) tex = self.make_wall_tile(self.texture_base[2], self.texture_base[3], self.texture_base[0], self.texture_base[1]) self.result.insert(5, tex) #: 6 self.result.insert(6, self.texture_base[1]) #: 7 tex = Texture2D(Point(Connected.res, Connected.res)) tex = self.make_wall_tile(self.texture_base[3], self.texture_base[3], self.texture_base[1], self.texture_base[1]) self.result.insert(7, tex) #: 8 tex = Texture2D(Point(Connected.res, Connected.res)) tex = self.make_wall_tile(self.texture_base[0], self.texture_base[0], self.texture_base[2], self.texture_base[2]) self.result.insert(8, tex) #: 9 self.result.insert(9, self.texture_base[2]) #: 10 tex = Texture2D(Point(Connected.res, Connected.res)) tex = self.make_wall_tile(self.texture_base[1], self.texture_base[0], self.texture_base[3], self.texture_base[2]) self.result.insert(10, tex) #: 11 tex = Texture2D(Point(Connected.res, Connected.res)) tex = self.make_wall_tile(self.texture_base[3], self.texture_base[2], self.texture_base[3], self.texture_base[2]) self.result.insert(11, tex) #: 12 tex = Texture2D(Point(Connected.res, Connected.res)) tex = self.make_wall_tile(self.texture_base[0], self.texture_base[1], self.texture_base[2], self.texture_base[3]) self.result.insert(12, tex) #: 13 tex = Texture2D(Point(Connected.res, Connected.res)) tex = self.make_wall_tile(self.texture_base[2], self.texture_base[3], self.texture_base[2], self.texture_base[3]) self.result.insert(13, tex) #: 14 tex = Texture2D(Point(Connected.res, Connected.res)) tex = self.make_wall_tile(self.texture_base[1], self.texture_base[1], self.texture_base[3], self.texture_base[3]) self.result.insert(14, tex) #: 6 self.result.insert(15, self.texture_base[3])