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])
