def pruning_test():
# Setup Team Object
team_edm = Team()
lines = list()
lines.append(Line(0, 1.701))
lines.append(Line(1, 0.658))
lines.append(Line(2, 0.299))
lines.append(Line(3, 0.433))
team_edm.set_team("EDM", lines=lines,
start_line_index=0) # Edmonton Oilers
path = Path()
path.team = team_edm
values_best = []
for i in range(30):
temp_path = path.copy()
optimal_path, num_visited = process(team_edm[team_edm.curr_line_index],
temp_path, 1, i)
values_best.append(num_visited)
path.team[path.team.curr_line_index].toi = 0
values_worst = []
for i in range(30):
temp_path = path.copy()
optimal_path, num_visited = process(team_edm[team_edm.curr_line_index],
temp_path, 1, i)
values_worst.append(num_visited)
theoretical_values = [theoretical_nodes(i) for i in range(30)]
print(theoretical_values)
print(values_best)
print(values_worst)
def alg4(Y): L = [] R = [] n = len(Y) l = int(math.floor(n/2)) print "n-l = " + repr(n-l) + " l= " +repr(l) for i in range(0,n-l): #print "i = " + repr(i) x = Line() L.append(x) L[i] = Y[i] for i in range(0,l): j = i + l x = Line() R.append(x) R[i] = Y[j] if(n <= 4): mergeLines(L,R) L.extend(R) return L else: for i in range(0,n-l): print "L[i] = " + repr(L[i].m) for j in range(0,l): print "R[j] = " + repr( R[j].m) mergeLines(alg4(L),alg4(R)) L.extend(R) return L
def fit_centroids(window_centroids, window_height, image_height, left_lane,
right_lane):
# return left and right fit given window_centroids(leftx, rightx) and centroid window_height
num_y_values = len(window_centroids)
y_values = []
left_x = []
right_x = []
for i in range(0, num_y_values):
y_values.append(image_height - (window_height * i))
left_x.append(window_centroids[i][0])
right_x.append(window_centroids[i][1])
#print("y:", y_values)
#print("left_x:", left_x)
#print("right_x:", right_x)
#left_fit = np.polyfit(y_values, left_x, 2)
#right_fit = np.polyfit(y_values, right_x, 2)
if left_lane == None:
# initialize the lanes
left_lane = Line(left_x, y_values, 720)
right_lane = Line(right_x, y_values, 720)
else:
# add this fit to the lanes
left_lane.fit_line(left_x, y_values)
right_lane.fit_line(right_x, y_values)
#print("left fit:", left_fit)
#print("right fit:", right_fit)
# Fit new polynomials to x,y in world space
# We can't simply scale curvature from pixel space since the y & x scales are different
#left_fit_cr = np.polyfit(np.asarray(y_values)*ym_per_pix, np.asarray(left_x)*xm_per_pix, 2)
#right_fit_cr = np.polyfit(np.asarray(y_values)*ym_per_pix, np.asarray(right_x)*xm_per_pix, 2)
return left_lane, right_lane
def __init__(self, width_mm, height_mm, rotation_ccw=0):
super().__init__()
self.rotation_ccw = rotation_ccw
# Horizontal
start_point = 0, 0
end_point = width_mm, 0
self.add(
self.NO_OFFSET,
Line(rotate(start_point, rotation_ccw),
rotate(end_point, rotation_ccw)))
start_point = 0, height_mm
end_point = width_mm, height_mm
self.add(
self.NO_OFFSET,
Line(rotate(start_point, rotation_ccw),
rotate(end_point, rotation_ccw)))
# Vertical
start_point = 0, 0
end_point = 0, height_mm
self.add(
self.NO_OFFSET,
Line(rotate(start_point, rotation_ccw),
rotate(end_point, rotation_ccw)))
start_point = width_mm, 0
end_point = width_mm, height_mm
self.add(
self.NO_OFFSET,
Line(rotate(start_point, rotation_ccw),
rotate(end_point, rotation_ccw)))
def find_all(self):
x_axis, y_axis = self.right_line.seperate_axis()
pol_right = np.polyfit(x_axis,y_axis,1)
rm = pol_right[0]
rb = pol_right[1]
x_axis, y_axis = self.left_line.seperate_axis()
pol_left = np.polyfit(x_axis,y_axis,1)
lm = pol_left[0]
lb = pol_left[1]
yb = self.HEIGHT
yt = yb/3.5
xt_r = (yt - rb) / rm;
xb_r = (yb - rb) / rm;
xt_l = (yt - lb) / lm;
xb_l = (yb - lb) / lm;
xb_center = (xb_r + xb_l)/2
xt_center = (xt_r + xt_l)/2
self.center_line = Line(xb_center,yb,xt_center,yt)
self.left_line = Line(xb_l,yb,xt_l,yt)
self.right_line = Line(xb_r,yb,xt_r,yt)
self.yaw = np.arctan(self.center_line.compute_slope())
self.center_line.draw(self.final_img,(255,0,0))
self.left_line.draw(self.final_img,(0,255,0))
self.right_line.draw(self.final_img,(0,255,0))
self.cte = self.center_line.x2 - self.center_line.x1
def populateLines(left_fit, right_fit, left_fitx, right_fitx, ploty,
left_curvature, right_curvature):
left_line = Line()
left_line.current_fit = left_fit
left_line.recent_xfitted.append(left_fitx)
if len(left_line.recent_xfitted) > 10:
left_line.recent_xfitted.remove(0)
left_line.last_fits.append(left_fit)
if len(left_line.last_fits) > 5:
left_line.last_fits.remove(0)
left_line.allx = left_fitx
left_line.ally = ploty
right_line = Line()
right_line.current_fit = right_fit
right_line.recent_xfitted.append(right_fitx)
if len(right_line.recent_xfitted) > 10:
right_line.recent_xfitted.remove(0)
right_line.last_fits.append(right_fit)
if len(right_line.last_fits) > 5:
right_line.last_fits.remove(0)
right_line.allx = right_fitx
right_line.ally = ploty
left_line.radius_of_curvature = left_curvature
right_line.radius_of_curvature = right_curvature
left_line.shift = left_fitx[500]
right_line.shift = right_fitx[500]
return left_line, right_line
def __init__(self, width, height, player, backgroundPath):
"""
:param width: width of the screen
:param height: height of the screen
:param player: instance of the player class
:param backgroundPath: background image path of the level
"""
self.player = player
self.bridge = pygame.image.load('./src/images/background/bridge.png')
#bridge position where the player will stand
self.bridgeYPosition = 190
self.background = pygame.image.load(backgroundPath)
#the lines are the limits where the objects that reach it will be removed
self.endingTopLine = Line(width, -400)
self.endingBottomLine = Line(width, height + 400)
#the factories are the generators of the objects on the screen
self.enemiesFactory = Factory(width, height)
self.treasureFactory = Factory(width, height)
self.attacksFactory = Factory(width, height)
self.livesFactory = Factory(width, height)
#this will be the counter of the escaping enemies
self.enemiesGoneCounter = TextOnScreen(50, 10, 18, (250, 250, 250),
'rockwell', "Escaped", 0)
#Player points counter that will be visible on the screen level
self.pointsCounter = TextOnScreen(700, 10, 18, (250, 250, 250),
'rockwell', "Points", player.points)
#Player lives counter that will be visible on the screen level
self.livesCounter = TextOnScreen(400, 10, 18, (250, 250, 250),
'rockwell', "Lives", player.lives)
def setup(self):
def process_sound(recognizer, audio_data):
try:
result = self.recognizer.recognize_google(audio_data)
except Exception:
return
force = 0.
if result.lower().startswith('l'):
force = -10000
elif result.lower().startswith('r'):
force = 10000
self.ball.push((force, 0), result)
self.stop_listening = self.recognizer.listen_in_background(
self.microphone,
process_sound,
phrase_time_limit=1.,
)
self.space = pymunk.Space()
self.space.gravity = 0.0, -900.0
self.bottom_line = Line(100, 100, 400, 100, self.space)
self.left_line = Line(100, 100, 100, 200, self.space)
self.right_line = Line(400, 100, 400, 200, self.space)
self.ball = Ball(200, 200, 10, self.space)
def compute_lane_from_candidates(line_candidates, img_shape):
"""
PURPOSE:
Compute lines that approximate the position of both road lanes.
INPUT:
line_candidates : Lines from hough transform
img_shape : Shape of image to which hough transform was applied on
OUTPUT:
left_lane : line that approximates left lane position
right_lane : line that approximates right lane position
"""
# separate candidate lines according to their slope
pos_lines = [l for l in line_candidates if l.slope > 0]
neg_lines = [l for l in line_candidates if l.slope < 0]
# interpolate biases and slopes to compute equation of line that approximates left lane
# median is employed to filter outliers
neg_bias = np.median([l.bias for l in neg_lines]).astype(int)
neg_slope = np.median([l.slope for l in neg_lines])
x1, y1 = 0, neg_bias
x2, y2 = -np.int32(np.round(neg_bias / neg_slope)), 0
left_lane = Line(x1, y1, x2, y2)
# interpolate biases and slopes to compute equation of line that approximates right lane
# median is employed to filter outliers
lane_right_bias = np.median([l.bias for l in pos_lines]).astype(int)
lane_right_slope = np.median([l.slope for l in pos_lines])
x1, y1 = 0, lane_right_bias
x2, y2 = np.int32(np.round((img_shape[0] - lane_right_bias) / lane_right_slope)), img_shape[0]
right_lane = Line(x1, y1, x2, y2)
return left_lane, right_lane
def get_distance_between(self, point1,
point2): #MODIFIED TO RETURN DISTANCE
"""
Calculates the distance between two given points when traversing edges
:param point1: tuple
:param point2: tuple
:return: integer
"""
edges = Border.get_reordered_edges(
self.get_edges(), point1) # Reorder edges st POINT 1 IS AT INDEX 0
try:
index_2 = Border.get_index_of(
edges, point2) # POINT 2 IS AT INDEX index_2
if index_2 == 0: # CASE A: points are on the same edge
modified_first_edge = Line(point1, point2)
distance = modified_first_edge.get_distance()
return distance
# CASE B: points are on different edges
modified_first_edge = Line(
point1, edges[0].endpoint2) # make modified edges
modified_last_edge = Line(edges[index_2].endpoint1, point2)
modified_edges = Trail(
) # make into trail (bc we have a function to calc distance)
modified_edges.lines = [modified_first_edge] + edges[1:index_2] + [
modified_last_edge
] # get disance
distance = modified_edges.get_distance()
return distance # = NUMBER OF LINES BTWN PT 1 AND 2; (+1 bc idx 0)
except: # index error, value error
return None # ex. If it is unable to find point2 for some reason and thus cannot do index2+1
def check_detected_lines(self, leftx, lefty, rightx, righty):
"""Check if the detected lane line pixels are plausible"""
is_valid_input = len(leftx) > 3 and len(rightx) > 3
lines_are_plausible = False
lines_are_parallel = False
if is_valid_input:
detected_left_line = Line(x=lefty, y=leftx)
detected_right_line = Line(x=righty, y=rightx)
# Check if they are parallel
first_coefficients_diff = np.abs(
detected_left_line.current_fit[2] -
detected_right_line.current_fit[2])
second_coefficients_diff = np.abs(
detected_left_line.current_fit[1] -
detected_right_line.current_fit[1])
lines_are_parallel = first_coefficients_diff < 0.0005 and second_coefficients_diff < 0.55
# Check if the lines have plausible distance
distance = np.abs(
detected_left_line.current_fit(719) -
detected_right_line.current_fit(719))
# print('Distance: ' + str(distance))
lines_are_plausible = 380 < distance < 550
detection_ok = is_valid_input & lines_are_plausible & lines_are_parallel
self.left_line.detected = detection_ok
self.right_line.detected = detection_ok
def testShotHit(shot):
x, y, z = shot.pos.Pos()
lx, ly, lz = shot.lpos.Pos()
sp = Vex(x, y)
lsp = Vex(lx, ly)
dmg = shot.damage / 100.0
for p in Planets.pl:
d = p.pos.dist2D(sp)
if d < p.atmos:
shln = Line(lsp - p.pos, sp - p.pos)
for lines2 in p.edges:
if lines2[1] != 1:
lines = Line(p.surfPoints[lines2[0][0]], p.surfPoints[lines2[0][1]])
li = shln.find_intersection(lines)
if li:
minR = 5
ParticleEngine.Emitter(li + p.pos, shot.angle_rad, 2)
if lines.seg[0].length() > p.rad - minR:
a1 = math.atan2(lines.seg[0].Y(), lines.seg[0].X())
d1 = ZERO.dist2D(lines.seg[0]) - dmg
lines.seg[0].set2D(math.cos(a1) * d1, math.sin(a1) * d1)
if lines.seg[1].length() > p.rad - minR:
a2 = math.atan2(lines.seg[1].Y(), lines.seg[1].X())
d2 = ZERO.dist2D(lines.seg[1]) - dmg
lines.seg[1].set2D(math.cos(a2) * d2, math.sin(a2) * d2)
return True
if d <= p.coreRad:
ParticleEngine.Emitter(Vex(x, y), shot.angle_rad, 2)
p.corHealth -= dmg * 10
if p.corHealth <= 0:
# do explody stuff
pass
return True
return False
def addLineToTextColumn(self, currentTextElementsList, textColumnsWidth):
"""
Creates a TextElement object and adds it to a line
"""
for xmlTextElement in currentTextElementsList:
currentTxtColumn = None
currentTextElement = self.createTextElement(xmlTextElement)
self.__rightColumn = abs(currentTextElement.getLeft() /
textColumnsWidth)
if (int(self.__rightColumn) < len(self.__textColumnsList)):
currentTxtColumn = self.__textColumnsList[int(
self.__rightColumn)]
if (len(currentTxtColumn.getLinesList())) > 0:
line = currentTxtColumn.getLine(
-1) #returns the value in the last position
if self.inTheLine(currentTextElement, line) == True:
# exactly in the boundaries of the line
line.addText(currentTextElement)
self.updateLineValues(currentTextElement, line)
else:
newLine = Line()
newLine.addText(currentTextElement)
self.setNewLineValues(currentTextElement, newLine)
currentTxtColumn.addLine(newLine)
self.__distance += int(newLine.getFirstTop()) - int(
line.getLastTop())
else:
newLine = Line()
newLine.addText(currentTextElement)
self.setNewLineValues(currentTextElement, newLine)
currentTxtColumn.addLine(newLine)
def add_edges(self, triangle, point): def index_of_other_triangle_in_its_side_edges(line, triangle): if line.triangle1 == triangle : return 1 return 2 l1, l2, l3 = triangle.sort_lines() line1 = Line(l1.point1, point) line2 = Line(l2.point1, point) line3 = Line(l3.point1, point) tri1 = Triangle(triangle.line1, line1, line2) tri2 = Triangle(triangle.line2, line3, line2) tri3 = Triangle(triangle.line3, line1, line3) line1.set_triangles(tri1, 1) line1.set_triangles(tri3, 2) line2.set_triangles(tri1, 1) line2.set_triangles(tri2, 2) line3.set_triangles(tri3, 1) line3.set_triangles(tri2, 2) triangle.line1.set_triangles(tri1, index_of_other_triangle_in_its_side_edges(triangle.line1, triangle)) triangle.line2.set_triangles(tri2, index_of_other_triangle_in_its_side_edges(triangle.line2, triangle)) triangle.line3.set_triangles(tri3, index_of_other_triangle_in_its_side_edges(triangle.line3, triangle)) triangle.triangles += [tri1, tri2, tri3] point.neighbours += [l1.point1, l2.point1, l3.point1] l1.point1.neighbours.append(point) l2.point1.neighbours.append(point) l3.point1.neighbours.append(point)
def __init__(self, screen_width=896, screen_height=504):
Environment.screen = pygame.display.set_mode(
(screen_width, screen_height), pygame.DOUBLEBUF)
Environment.screen_width = screen_width
Environment.screen_height = screen_height
Environment.center_x = screen_width / 2
Environment.center_y = screen_height / 2
Environment.center = Vex(Environment.center_x, Environment.center_y)
# left
Environment.LEFT = Line(Vex(0, 0), Vex(0, screen_height))
# right
Environment.RIGHT = Line(Vex(screen_width, 0),
Vex(screen_width, screen_height))
# top
Environment.TOP = Line(Vex(0, 0), Vex(screen_width, 0))
# bottom
Environment.BOTTOM = Line(Vex(0, screen_height),
Vex(screen_width, screen_height))
### Bind Screen ###
Environment.bounds += [
Environment.LEFT, Environment.RIGHT, Environment.TOP,
Environment.BOTTOM
]
# Create SOl
Environment.refreshSOL()
Objects.Objects.init()
DungeonGenerator.init()
def testRadiusCollision(ent, cp, np, rad):
ent.inatmosphere = False
for p in Planets.pl:
d = (cp + np).dist2D(p.pos)
if d <= p.atmos:
edit = False
if d > p.coreRad - 6:
ent.inatmosphere = True
v = (cp + np)
out = Vex(0, 0)
delta = v - p.pos # new position relative to planet position
delta0 = cp - p.pos
for lines2 in p.edges:
lines = (Line(p.surfPoints[lines2[0][0]], p.surfPoints[lines2[0][1]]), lines2[1])
if lines[1] == 0 or (lines[1] == 1 and ent.inship):
ds = lines[0].ClosestPointOnLine(delta0)
cpol = delta.dist2D(ds)
if cpol <= rad:
# print (lines[0])
a = -(lines[0].angle()) + math.pi
v = Vex(math.sin(a) * rad, math.cos(a) * rad) + ds + p.pos
ParticleEngine.Emitter(v, a, 3)
ParticleEngine.Emitter(ds + p.pos, a, 2)
out.assign(v)
edit = True
break
if not edit:
for segs in p.entryPs:
ln = []
ln += [Line(p.surfPoints[segs[0]], p.sancPoints[segs[0]])]
ln += [Line(p.sancPoints[segs[1]], p.surfPoints[segs[1]])]
for i in range(0, 2):
lines = ln[i]
ds = lines.ClosestPointOnLine(delta)
cpol = delta.dist2D(ds)
if cpol <= rad:
a = -(lines.angle()) + math.pi
v = Vex(math.sin(a) * rad, math.cos(a) * rad) + ds + p.pos
ParticleEngine.Emitter(ds + p.pos, a, 2)
out.assign(v)
edit = True
# if d < p.rad:
# if not edit:
# for lines2 in p.edges:
# lines = (Line(p.sancPoints[lines2[0][1]], p.sancPoints[lines2[0][0]]), lines2[1])
# if lines[1] == 0 or (lines[1] == 1 and ent.inship):
# ds = lines[0].ClosestPointOnLine(delta)
# cpol = delta.dist2D(ds)
# if cpol <= rad:
# a = -(lines[0].angle()) + math.pi
# v = Vex(math.sin(a) * rad, math.cos(a) * rad) + ds + p.pos
# ParticleEngine.Emitter(ds + p.pos, a, 2)
# out.assign(v)
# edit = True
if edit:
return out
return None
def line_possibility(self, left, right):
if len(left[0]) < 3 or len(right[0]) < 3:
return False
else:
new_left = Line(y=left[0], x=left[1])
new_right = Line(y=right[0], x=right[1])
return are_lanes_plausible(new_left, new_right)
def __init__(self, cal_files=[]):
self.camera = Camera()
self.camera.calibrate(cal_files)
temp = cv2.imread(cal_files[0])
_, _ = self.camera.calibrate_perspective(temp)
self.img_shape = temp.shape
self.left_lane = Line(self.img_shape)
self.right_lane = Line(self.img_shape)
def offset(self, default_thickness=None):
points = []
self.reorder_sectors()
if default_thickness:
thck = [float(default_thickness) for i in range(len(self))]
elif hasattr(self, 'thick'):
thck = self.thick + [self.thick[0]]
if len(self) > 2:
order = self[1:] + [self[1]]
print '--ordered points--'
print order
print '--sectors--'
sectors = [(order[i], self.center, order[i + 1], thck[i],
thck[i + 1]) for i in range(len(order) - 1)]
print sectors
print '----'
#prev = None
for isec, s in enumerate(sectors):
#print s
v1 = Vector([s[0][i] - s[1][i] for i in range(3)]).unit()
v2 = Vector([s[2][i] - s[1][i] for i in range(3)]).unit()
t1 = cross(Vector([0., 0., 1.]), v1).unit()
t2 = cross(v2, Vector([0., 0., 1.])).unit()
p1 = Point([s[0][i] + 0.5 * s[3] * t1[i] for i in range(3)])
p2 = Point([s[2][i] + 0.5 * s[4] * t2[i] for i in range(3)])
li1 = Line([p1, v1])
li2 = Line([p2, v2])
p0 = intersect_2_lines(li1, li2)
#gp1 = p1.pop_to_geom(geom)
#gp2 = p2.pop_to_geom(geom)
#if prev:
# gprev = prev.pop_to_geom(geom)
# geom.add_line(gprev,gp1)
if p0:
#gp0 = p0.pop_to_geom(geom)
#geom.add_line(gp1,gp0)
#geom.add_line(gp0,gp2)
#plt.plot([p1[0], p0[0], p2[0]],
# [p1[1], p0[1], p2[1]])
points.append(p1)
points.append(p0)
points.append(p2)
else:
points.append(p1)
points.append(p2)
#geom.add_line(gp1,gp2)
else:
th = thck[0]
v1 = Vector([self[1][i] - self[0][i] for i in range(3)]).unit()
t1 = cross(Vector([0., 0., 1.]), v1).unit()
p1 = Point([self[1][i] + 0.5 * th * t1[i] for i in range(3)])
p2 = Point([self[0][i] + 0.5 * th * t1[i] for i in range(3)])
p3 = Point([self[0][i] - 0.5 * th * t1[i] for i in range(3)])
p4 = Point([self[1][i] - 0.5 * th * t1[i] for i in range(3)])
points = [p1, p2, p3, p4]
return Polyline2D(points, closed=True)
def main():
"""
Sets up and processes team, EXAMPLE;
:return: NONE
"""
# Setup Team Object
team_edm = Team()
lines = list()
lines.append(Line(0, 1.701))
lines.append(Line(1, 0.658))
lines.append(Line(2, 0.299))
lines.append(Line(3, 0.433))
team_edm.set_team("EDM", lines=lines,
start_line_index=0) # Edmonton Oilers
print(team_edm)
# Setup Path Object
path = Path()
path.team = team_edm
schedule = []
num_intervals = PERIOD_LENGTH // INTERVAL
start = time.time()
for i in range(num_intervals - 1):
max_depth = MAX_DEPTH
if num_intervals - i < MAX_DEPTH:
max_depth = num_intervals - i
# start = time.time()
# find optimal path from curr_line for the next (MAX_DEPTH * INTERVAL) seconds
temp_path = path.copy()
optimal_path, num_visited = process(team_edm[team_edm.curr_line_index],
temp_path, 1, max_depth)
# print("\n\n", path.team, "\n\n")
path.team.update(optimal_path[1], INTERVAL)
schedule.append(optimal_path[1].line_num)
elapsed_time = time.time() - start
t_nodes = theoretical_nodes(max_depth)
# print("Optimal ", optimal_path)
#
print("Progress: {p:3.0f}% @ t: {e:5.2f}".format(
p=i / (num_intervals - 1) * 100, e=elapsed_time))
# print("Look Ahead: ", LOOK_AHEAD)
# print("Depth: ", max_depth)
# print("Visited: ", num_visited)
# print("Theoretical: ", t_nodes)
# print("Removal Rate: ", "{0:0.5f}".format((t_nodes - num_visited)/t_nodes))
# print("Speed Up: ", "{0:4.2f}".format(t_nodes/num_visited))
#
# print("\nTime: ", elapsed_time)
print(schedule)
def __detect_lane(self, warped_thresh_image, orig_frame):
"""Detect lanes lines in the undisorted original image 'orig_frame' using the warped binary threshold image
'warped_thres_image'"""
left_points = []
right_points = []
left_detected = right_detected = False
# 1. Create a histogram for the bottom half of the image and determine the peaks of both lane lines
histogram = np.sum(warped_thresh_image[int(warped_thresh_image.shape[0] / 2):, :], axis=0)
center_x = np.int(histogram.shape[0] / 2)
left_peak = np.argmax(histogram[:center_x])
right_peak = np.argmax(histogram[center_x:]) + center_x
# Add the two detected peaks as base points to the array, they serve as sliding window starting points
left_points.append((left_peak, warped_thresh_image.shape[0]))
right_points.append((right_peak, warped_thresh_image.shape[0]))
# 2. Use a sliding window to find all lane line centres in the image for both lane lines
img_height = warped_thresh_image.shape[0]
window_height = int(img_height / PEAK_SLIDING_WINDOW_NUM) # height of each sliding window
detected_left = self.__sliding_window(left_points, warped_thresh_image, img_height, window_height)
detected_right = self.__sliding_window(right_points, warped_thresh_image, img_height, window_height)
# 3. Determine whether proper lane lines were detected by comparing the left and right line curvature and
# distance between the two lines
if not left_detected or not right_detected:
left_detected, right_detected = self.__validate_lines((detected_left[:, 0], detected_left[:, 1]),
(detected_right[:, 0], detected_right[:,1]))
# 4. Update line information
if left_detected:
if self.left_line is not None:
self.left_line.update(y = detected_left[:,0], x = detected_left[:,1])
else:
self.left_line = Line(self.n_frames,
detected_y = detected_left[:, 0],
detected_x = detected_left[:, 1])
if right_detected:
if self.right_line is not None:
self.right_line.update(y = detected_right[:,0], x = detected_right[:,1])
else:
self.right_line = Line(self.n_frames,
detected_y = detected_right[:,0],
detected_x = detected_right[:,1])
# 5. Draw the lane and add additional information
if self.left_line is not None and self.right_line is not None:
self.curvature_left = calc_curvature(self.left_line.best_fit_poly)
self.curvature_right = calc_curvature(self.right_line.best_fit_poly)
self.center_poly = (self.left_line.best_fit_poly + self.right_line.best_fit_poly) / 2
self.offset = (orig_frame.shape[1] / 2 - self.center_poly(IMAGE_MAX_Y)) * XM_PER_PIXEL
orig_frame = self.__plot_lane(orig_frame, warped_thresh_image)
return orig_frame
def cut(self, line):
a = line.getSize() / (2 * math.sqrt(3))
t = line.getTurn() - math.pi / 2
pMid = line.getPoint(0.5)
xScale = a * math.cos(t)
yScale = a * math.sin(t)
pOne = Point(pMid.X() + xScale, pMid.Y() + yScale)
pTwo = Point(pMid.X() - xScale, pMid.Y() - yScale)
p = [line.getStart(), pOne, pTwo, line.getEnd()]
return [Line(p[0], p[1]), Line(p[1], p[2]), Line(p[2], p[3])]
def __init__(self):
self.bridge = CvBridge()
self.sub_image = rospy.Subscriber('camera/image_raw', Image, self.img_callback, queue_size=1)
self.pub_image = rospy.Publisher("lane_detection/annotate", Image, queue_size=1)
self.pub_bird = rospy.Publisher("lane_detection/birdseye", Image, queue_size=1)
self.left_line = Line(n=5)
self.right_line = Line(n=5)
self.detected = False
self.hist = True
def __init__(self):
self.__leftLine, self.__rightLine = Line(LineType.left), Line(
LineType.right)
self.__nwindows = 8
# Set the width of the windows +/- margin
self.__margin = 45
# Set minimum number of pixels found to recenter window
self.__minpix = 50
self.__imageUtils = ImageUtils()
def __create(self):
one = Point(-self.a / 2, self.b / 2, 0)
two = Point(self.a / 2, self.b / 2, 0)
three = Point(self.a / 2, -self.b / 2, 0)
four = Point(-self.a / 2, -self.b / 2, 0)
five = Point(-self.c / 2, 0, self.h)
six = Point(self.c / 2, 0, self.h)
line12 = Line(one, two)
line14 = Line(one, four)
line15 = Line(one, five)
line23 = Line(two, three)
line26 = Line(two, six)
line34 = Line(three, four)
line36 = Line(three, six)
line45 = Line(four, five)
line56 = Line(five, six)
plane1234 = Plane([line12, line14, line23, line34], one, two, three)
plane154 = Plane([line15, line14, line45], one, five, four)
plane236 = Plane([line23, line26, line36], two, three, six)
plane1265 = Plane([line12, line15, line56, line26], one, two, five)
plane5643 = Plane([line56, line36, line34, line45], five, four, three)
self.planes.append(plane1234)
self.planes.append(plane154)
self.planes.append(plane236)
self.planes.append(plane1265)
self.planes.append(plane5643)
def makeUsable(self):
# get the angle for the hall using rooms center position
raP = self.rooms[0].centerPos
rbP = self.rooms[1].centerPos
a = -raP.ang2D(rbP) + math.pi
# calculate the hall points
self.dim += [
raP + Vex(math.sin(a) * (self.w / 2),
math.cos(a) * (self.w / 2))
]
self.dim += [
raP - Vex(math.sin(a) * (self.w / 2),
math.cos(a) * (self.w / 2))
]
self.dim += [
rbP - Vex(math.sin(a) * (self.w / 2),
math.cos(a) * (self.w / 2))
]
self.dim += [
rbP + Vex(math.sin(a) * (self.w / 2),
math.cos(a) * (self.w / 2))
]
# get all the intersection points of rooms
la = []
k = len(self.rooms[0].dim) - 1
for ri in range(len(self.rooms[0].dim)):
r = Line(self.rooms[0].dim[k], self.rooms[0].dim[ri])
j = len(self.dim) - 1
for hi in range(len(self.dim)):
fip = r.find_intersection(Line(self.dim[hi], self.dim[j]))
if fip:
la += [((k, ri), (hi, j), fip)]
j = hi
k = ri
lb = []
k = len(self.rooms[1].dim) - 1
for ri in range(len(self.rooms[1].dim)):
r = Line(self.rooms[1].dim[k], self.rooms[1].dim[ri])
j = len(self.dim) - 1
for hi in range(len(self.dim)):
fip = r.find_intersection(Line(self.dim[hi], self.dim[j]))
if fip:
lb += [((k, ri), (hi, j), fip)]
j = hi
k = ri
# adjust points to intersection
print(str(raP) + " " + str(la))
for l in la:
self.rooms[0].dim.insert(l[0][1], l[2])
self.dim[l[1][1]].assign(l[2])
for l in lb:
self.rooms[1].dim.insert(l[0][1], l[2])
def __create_axes_oy(self):
x = self.__width / 2 + self.__travel.calculate_travel_x()
start = Point(x, 0)
end = Point(x, self.__height)
start_arrow_line_up = Point(x - 5, 10)
start_arrow_line_down = Point(x + 5, 10)
return [
Line(start, end),
Line(start_arrow_line_up, start),
Line(start_arrow_line_down, start)
]
def generate_endpoints(endpoints, current, min_size=0.2):
if current.radius() > min_size:
endpoints[0].append(current.x1)
endpoints[0].append(current.x2)
endpoints[1].append(1)
endpoints[1].append(1)
center = current.center()
left = center - current.radius() * SQRT2
right = center + current.radius() * SQRT2
generate_endpoints(endpoints, Line(left, center), min_size=min_size)
generate_endpoints(endpoints, Line(center, right), min_size=min_size)
def __init__(self):
'''
Initializes the course map
'''
super(TestMap, self).__init__()
# Course Circles
self.circles = []
# Course Lines
self.t1 = Line(TestMap.T1_START_POINT, TestMap.T1_END_POINT)
self.t2 = Line(TestMap.T2_START_POINT, TestMap.T2_END_POINT)
self.lines = [self.t1, self.t2]
def __init__(self, cpos, rect=[0, 0, 0, 0]):
self.centerPos = cpos
self.dim = rect
self.edges = [Line(self.dim[0], self.dim[1]), Line(self.dim[1], self.dim[2]), \
Line(self.dim[2], self.dim[3]), Line(self.dim[3], self.dim[0])]
self.edgesI = [(0, 1), (1, 2), (2, 3), (3, 0)]
self.visible = True
self.halls = []
self.obj = []
self.traps = []
self.npc = []
