-
Notifications
You must be signed in to change notification settings - Fork 2
/
draw_beh.py
executable file
·252 lines (214 loc) · 8.12 KB
/
draw_beh.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
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
import pdb
import time
import cv2
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from skip import skip
'''
Canny Edge Detector: http://bigwww.epfl.ch/demo/ip/demos/edgeDetector/
Invert the color: https://pinetools.com/invert-image-colors
'''
class Drawer(object):
"""Instance of this class find the best path for inverse kinematics
# Constructor Arguments:
filename: input image absolute filepath.
h_draw: height of end-effector while drawing.
h_move: height of end-effector while moving.
# Methods:
find_closest
findPathUtils
findPath
draw
"""
@staticmethod
def find_closest(pts, point):
"""This static method find the closest pixel from the current pixel to continue
drawing when the current pixel does not have any neighbouring pixel with value 0
# Arguments:
pts: all remaining pixels' coordinates.
point: the current pixel coordinates.
"""
pts_arr = np.array(list(pts))
dist_2 = np.sum((pts_arr - point) ** 2, axis=1)
return pts_arr[np.argmin(dist_2)]
def __init__ (self, filename, h_draw, h_move,display):
self.h_draw = h_draw
self.h_move = h_move
self.display = display
# Read image from path in grayscale
self.arr = cv2.imread(filename, cv2.IMREAD_GRAYSCALE)
(h, w) = self.arr.shape[:2]
(cX, cY) = (w / 2, h / 2)
# grab the rotation matrix (applying the negative of the
# angle to rotate clockwise), then grab the sine and cosine
# (i.e., the rotation components of the matrix)
M = cv2.getRotationMatrix2D((cX, cY), 90, 1.0)
cos = np.abs(M[0, 0])
sin = np.abs(M[0, 1])
# compute the new bounding dimensions of the image
nW = int((h * sin) + (w * cos))
nH = int((h * cos) + (w * sin))
# adjust the rotation matrix to take into account translation
M[0, 2] += (nW / 2) - cX
M[1, 2] += (nH / 2) - cY
# perform the actual rotation and return the image
self.arr = cv2.warpAffine(self.arr, M, (nW, nH))
# Convert gray pixels into black
self.arr[self.arr < 150] = 0
# Find all black pixels
self.pts = set(map(tuple, np.argwhere(self.arr == 0)))
def findPathUtils(self, y, x):
"""Recursively (Depth-first search approach) visit remaining pixels.
# Arguments:
y: row index (height) of pixel
x: column index (width) of pixel
# Return:
None
"""
if self.point is None:
self.path.append((y, x, self.h_move))
else:
self.pts.remove((y, x))
if self.path[-1][2] == self.h_move:
self.path.append((y, x, self.h_move))
self.path.append((y, x, self.h_draw))
self.point = (y, x)
self.arr[y][x] = 255 # Mark pixel as visited
flag = False
# Check out all 8 neighbouring pixels
for i in range(-1, 2):
for j in range(-1, 2):
try: # Handle IndexError exceptions (when pixel is at edge of image)
if self.arr[y + i][x + j] == 0 and (y + i, x + j) in self.pts: # If pixel is not visited
self.findPathUtils(y + i, x + j)
flag = True
except Exception as e:
# print(e)
pass
if not flag:
self.path.append((y, x, self.h_move))
def findPath(self):
"""Find the path (to-be-drawn pixels coordinates in specific order) that optimize
inverse kinematics computation.
# Arguments:
None
# Return:
None
"""
self.point = None
self.path = []
while (self.pts): # While there are still points we have not visited
# print(len(self.pts))
if self.point is None: # If the first point is visited
y, x = self.pts.pop()
else:
# When we visit a pixel with non-zero neighbours, we find the
# closest remaining pixel to continue drawing.
y, x = self.find_closest(self.pts, self.point)
self.findPathUtils(y, x)
def draw(self):
if(self.display):
cv2.namedWindow("test", cv2.WINDOW_NORMAL)
a = 0
for y, x, h in self.path:
a +=1
print(a)
print("y, x, h = {}, {}, {}".format(y, x, h))
self.arr[y, x] = 0
cv2.imshow("test", self.arr)
key = cv2.waitKey(10) # Pause for 3 seconds before fetching next image
if key == 27: # If ESC is pressed, exit loop
cv2.destroyAllWindows()
break
cv2.imwrite("output.jpg",self.arr)
return self.path
def plot(arr,plot,plot_2D):
'''
arr: array that need to be plotted out
plot: set True to display the ploy
plot_2D: set True to plot the 2D diagram, else 3D
'''
y=[]
x=[]
z=[]
two_D_plot = plot_2D
for i in arr:
y.append(i[0])
x.append(i[1])
z.append(i[2])
max_y = max(y)
min_y = min(y)
max_x = max(x)
min_x = min(x)
print("In x-axis, Max: {} Min: {} Offset: {}.".format(max_x,min_x,(max_x+min_x)/2))
print("In y-axis, Max: {} Min: {} Offset: {}.".format(max_y,min_y,(max_y+min_y)/2))
if plot:
if two_D_plot:
plt.title('Output Points forom Drawing')
plt.scatter([i-(max_y+min_y)/2 for i in y], [i+(max_x+min_x)/2 for i in x])
# plt.scatter(y, x)
else:
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.plot(arr[:,1],arr[:,0],arr[:,2])
ax.scatter(arr[0,1],arr[0,0],arr[0,2],marker="o") # Starting Point
ax.scatter(arr[-1,1],arr[-1,0],arr[-1,2],marker="x") # Ending Point
plt.show()
return (max_y+min_y)/2,(max_x+min_x)/2
def main():
H_move = 6
H_draw = -1.7
filename = "Image/image.png"
# filename = "Image/abc.jpeg"
drawer = Drawer(filename, H_draw,H_move,False)
drawer.findPath()
arr = drawer.draw()
arr=np.asarray(arr) # <type 'numpy.ndarray'>
# Factor x coordinate & y coordinate
for i in arr:
i[0] = i[0]*0.05
i[1] = i[1]*0.05
arr = np.round(arr,1)
print("Number of point to IK: {}".format(len(arr)))
arr = arr.tolist()
yk = False
if yk:
arr = skip(arr,drawer.h_move,drawer.h_draw)
else:
reject=[]
counter = 0
for index,i in enumerate(arr):
if i[2]==drawer.h_move:
print("test")
else:
# print(counter)
counter+=1 # Issue that I mentioned yesterday, the ending part issue
if(counter!=30 and (arr[index+1])[2]!=drawer.h_move and (arr[index-1])[2]!=drawer.h_move):
reject.append(index) # save the index so that can use the np.deleted() function
else:
counter=0 # keep the tenth point and reset the counter
arr=np.asarray(arr) # convert list to array so that can use np.delete()
arr=np.delete(arr,reject,axis=0) # (original array, index that need to be deleted, axis)
# print(arr)
arr=np.asarray(arr)
print("After F Number of point to IK: {}".format(len(arr)))
'''
Workspace Region
'''
# min_X = 10
# max_X = 30
# min_Y =-15
# max_Y = 15
# arr = [[min_X,max_Y,H_move],
# [min_X,max_Y,H_draw],
# [min_X,min_Y,H_draw],
# [max_X,min_Y,H_draw],
# [max_X,max_Y,H_draw],
# [min_X,max_Y,H_draw],
# [min_X,max_Y,H_move]]
# arr=np.asarray(arr)
plot(arr,True,True) # set True to plot 2D graph
plot(arr,True,False) # set True to plot 2D graph
if __name__ == "__main__":
main()