-
Notifications
You must be signed in to change notification settings - Fork 0
/
route_finder.py
171 lines (155 loc) · 5.69 KB
/
route_finder.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
import cv2
import numpy as np
class Vertex:
def __init__(self,x_coord,y_coord):
self.x=x_coord
self.y=y_coord
self.d=float('inf') #distance from source
self.parent_x=None
self.parent_y=None
self.processed=False
self.index_in_queue=None
def get_neighbors(mat,r,c):
shape=mat.shape
neighbors=[]
#ensure neighbors are within image boundaries
if r > 0 and not mat[r-1][c].processed:
neighbors.append(mat[r-1][c])
if r < shape[0] - 1 and not mat[r+1][c].processed:
neighbors.append(mat[r+1][c])
if c > 0 and not mat[r][c-1].processed:
neighbors.append(mat[r][c-1])
if c < shape[1] - 1 and not mat[r][c+1].processed:
neighbors.append(mat[r][c+1])
return neighbors
def bubble_up(queue, index):
if index <= 0:
return queue
p_index=(index-1)//2
if queue[index].d < queue[p_index].d:
queue[index], queue[p_index]=queue[p_index], queue[index]
queue[index].index_in_queue=index
queue[p_index].index_in_queue=p_index
queue = bubble_up(queue, p_index)
return queue
def bubble_down(queue, index):
length=len(queue)
lc_index=2*index+1
rc_index=lc_index+1
if lc_index >= length:
return queue
if lc_index < length and rc_index >= length: #just left child
if queue[index].d > queue[lc_index].d:
queue[index], queue[lc_index]=queue[lc_index], queue[index]
queue[index].index_in_queue=index
queue[lc_index].index_in_queue=lc_index
queue = bubble_down(queue, lc_index)
else:
small = lc_index
if queue[lc_index].d > queue[rc_index].d:
small = rc_index
if queue[small].d < queue[index].d:
queue[index],queue[small]=queue[small],queue[index]
queue[index].index_in_queue=index
queue[small].index_in_queue=small
queue = bubble_down(queue, small)
return queue
def get_distance(img,u,v):
return 0.1 + (float(img[v][0])-float(img[u][0]))**2+(float(img[v][1])-float(img[u][1]))**2+(float(img[v][2])-float(img[u][2]))**2
def get_distance(img,u,v):
return 0.1 + (float(img[v][0])-float(img[u][0]))**2+(float(img[v][1])-float(img[u][1]))**2+(float(img[v][2])-float(img[u][2]))**2
def drawPath(img,path, thickness=3):
'''path is a list of (x,y) tuples'''
x0,y0=path[0]
for vertex in path[1:]:
x1,y1=vertex
cv2.line(img,(x0,y0),(x1,y1),(255,255,255),thickness)
x0,y0=vertex
def colorPath(img,path, thickness=1):
'''path is a list of (x,y) tuples'''
x0,y0=path[0]
for vertex in path[1:]:
x1,y1=vertex
cv2.line(img,(x0,y0),(x1,y1),(0,0,255),thickness)
x0,y0=vertex
def find_shortest_path(img,src,dst):
pq=[] #min-heap priority queue
source_x=src[0]
source_y=src[1]
dest_x=dst[0]
dest_y=dst[1]
imagerows,imagecols=img.shape[0],img.shape[1]
matrix = np.full((imagerows, imagecols), None) #access by matrix[row][col]
for r in range(imagerows):
for c in range(imagecols):
matrix[r][c]=Vertex(c,r)
matrix[r][c].index_in_queue=len(pq)
pq.append(matrix[r][c])
matrix[source_y][source_x].d=0
pq=bubble_up(pq, matrix[source_y][source_x].index_in_queue)
while len(pq) > 0:
u=pq[0]
u.processed=True
pq[0]=pq[-1]
pq[0].index_in_queue=0
pq.pop()
pq=bubble_down(pq,0)
neighbors = get_neighbors(matrix,u.y,u.x)
for v in neighbors:
dist=get_distance(img,(u.y,u.x),(v.y,v.x))
if u.d + dist < v.d:
v.d = u.d+dist
v.parent_x=u.x
v.parent_y=u.y
idx=v.index_in_queue
pq=bubble_down(pq,idx)
pq=bubble_up(pq,idx)
path=[]
iter_v=matrix[dest_y][dest_x]
path.append((dest_x,dest_y))
while(iter_v.y!=source_y or iter_v.x!=source_x):
path.append((iter_v.x,iter_v.y))
iter_v=matrix[iter_v.parent_y][iter_v.parent_x]
path.append((source_x,source_y))
return path
def find_nearest_white(img, target):
nonzero = cv2.findNonZero(img)
distances = np.sqrt((nonzero[:,:,0] - target[0]) ** 2 +
(nonzero[:,:,1] - target[1]) ** 2)
nearest_index = np.argmin(distances)
return nonzero[nearest_index]
def off_road_paths(start_point, road_start, end_point, road_end):
pth1=[]
pth2=[]
pth1.append(start_point.copy())
while (start_point[0] != road_start[0]):
while (start_point[0] > road_start[0]):
start_point[0] -=1
pth1.append(start_point.copy())
while (start_point[0] < road_start[0]):
start_point[0] +=1
pth1.append(start_point.copy())
while (start_point[1] != road_start[1]):
while (start_point[1] > road_start[1]):
start_point[1] -=1
pth1.append(start_point.copy())
while (start_point[1] < road_start[1]):
start_point[1] +=1
pth1.append(start_point.copy())
pth2.append(end_point.copy())
while (end_point[0] != road_end[0]):
while (end_point[0] > road_end[0]):
end_point[0] -=1
pth2.append(end_point.copy())
while (end_point[0] < road_end[0]):
end_point[0] +=1
pth2.append(end_point.copy())
while (end_point[1] != end_point[1]):
while (end_point[1] > road_end[1]):
end_point[1] -=1
pth2.append(end_point.copy())
while (end_point[1] < road_end[1]):
end_point[1] +=1
pth2.append(end_point.copy())
pth2 = pth2[::-1]
return pth1, pth2