/
trackRedSailMarkers.py
325 lines (250 loc) · 10.8 KB
/
trackRedSailMarkers.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
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
import cv2
import numpy as np
import matplotlib.pyplot as plt
import pprint
import math
import pickle
import scipy.stats as stats
testNum = 29
filename = 'Data1/t'+str(testNum)+'.mp4'
numberErrorsBk = 0
centerBk = 0; radiusBk = 0
centerPBk = 0; radiusPBk = 0;
numberErrorsFt = 0
centerPFt = 0; radiusPFt = 0;
frameNum = 0
Lbk = 240 #length of back flag, in pixels
centerArrayA = []; centerArrayB = [] ## Front Flag (red marker)
centerArrayX = []; centerArrayY = [] ## Back Flag (green marker)
thetaFlagFt = []; thetaFlagbk = [];
def nothing(x):
pass
#Green
hlg = 40
slg = 45
vlg = 80
hhg = 100
shg = 255
vhg = 255
#Red
hlr = 150
slr = 45
vlr = 80
hhr = 255
shr = 255
vhr = 255
cv2.namedWindow('greenHSVMask')
# create trackbars for color change
cv2.createTrackbar('H_low_green','greenHSVMask',hlg,180,nothing)
cv2.createTrackbar('S_low_green','greenHSVMask',slg,255,nothing)
cv2.createTrackbar('V_low_green','greenHSVMask',vlg,255,nothing)
cv2.createTrackbar('H_high_green','greenHSVMask',hhg,180,nothing)
cv2.createTrackbar('S_high_green','greenHSVMask',shg,255,nothing)
cv2.createTrackbar('V_high_green','greenHSVMask',vhg,255,nothing)
cv2.namedWindow('redHSVMask')
# create trackbars for color change
cv2.createTrackbar('H_low_red','redHSVMask',hlr,180,nothing)
cv2.createTrackbar('S_low_red','redHSVMask',slr,255,nothing)
cv2.createTrackbar('V_low_red','redHSVMask',vlr,255,nothing)
cv2.createTrackbar('H_high_red','redHSVMask',hhr,180,nothing)
cv2.createTrackbar('S_high_red','redHSVMask',shr,255,nothing)
cv2.createTrackbar('V_high_red','redHSVMask',vhr,255,nothing)
def posToTheta(i):
angle = math.acos( (Lbk - (max(centerArrayX) - i) )/Lbk )
print angle
return angle
def reject_outliers(data, m=2):
""" From:
http://stackoverflow.com/questions/11686720/is-there-a-numpy-builtin-to-reject-outliers-from-a-list
"""
return data[abs(data - np.mean(data)) < m * np.std(data)]
def colorImgPreProcess(image):
"""
Prepare images to be analyzed in binary form by appling generic filtering.
This makes them easier to work with and the resulting image less noisy.
INPUT: image for pre-processing. Should be in color, though b&w should work.
OUTPUT: returns a RGB image which has been filtered and looks nicer.
"""
#do processing on the image while it's still in color
image = cv2.medianBlur(image, 3) #kernal size must be odd
#image = cv2.bilateralFilter(image, 9, 75, 75) #TODO: uncomment when it won't cause C++ errors with ROS
#self.closeImages() #uncomment if showing output image
return image
def make_image_mask_green(img, a=40, b=10, c=10, d=80, e=200, f=255):
# B G R
#lower_blue = np.array([10,70,10], dtype=np.uint8)# np.array([60,30,55])
#upper_blue = np.array([150,250,60], dtype=np.uint8)
hsv_img = cv2.cvtColor(img,cv2.COLOR_BGR2HSV)
# Threshold the HSV image to get only blue colors
img = colorImgPreProcess(hsv_img)
lower_bound = np.array([a, b, c], dtype=np.uint8)
upper_bound = np.array([d, e, f], dtype=np.uint8)
print 'upper_bound: ', upper_bound
try:
thresh = cv2.inRange(img, lower_bound, upper_bound)
#cv2.imshow('green mask', thresh)
return thresh
except:
print "Fatal inRange error in make_image_mask_green!"
return ''
def make_image_mask_red(img, a=40, b=10, c=10, d=80, e=200, f=255):
hsv_img = cv2.cvtColor(img,cv2.COLOR_BGR2HSV)
img = colorImgPreProcess(hsv_img)
lower_bound = np.array([a, b, c], dtype=np.uint8)
upper_bound = np.array([d, e, f], dtype=np.uint8)
print 'red lower bound: ', lower_bound
print 'red upper bound: ', upper_bound
try:
thresh = cv2.inRange(img, lower_bound, upper_bound)
#cv2.imshow('Red Mask', thresh)
return thresh
except:
print "Fatal inRange error in make_image_mask_red!"
return ''
def make_image_mask_red_RGB(img):
# B G R
lower_blue = np.array([23,0,80], dtype=np.uint8)# np.array([60,30,55])
upper_blue = np.array([100,80,255], dtype=np.uint8)
# Threshold the HSV image to get only blue colors
try:
thresh = cv2.inRange(img, lower_blue, upper_blue)
#cv2.imshow('Red RGB mask', thresh)
return thresh
except:
print "Fatal inRange error in make_image_mask_red_RGB!"
return ''
def get_red_circle(frame, hlr, slr, vlr, hhr, shr, vhr):
global numberErrorsFt
global centerFt
global radiusFt
side_mask = make_image_mask_red(frame, hlr, slr, vlr, hhr, shr, vhr)
if type(side_mask) != type(''):
contours, hierarchy = cv2.findContours(side_mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
if len(contours):
cnt = contours[0]
(x,y),radiusFt = cv2.minEnclosingCircle(cnt)
centerFt = (int(x),int(y))
radiusFt = int(radiusFt)
side_mask = cv2.cvtColor(side_mask, cv2.COLOR_GRAY2RGB)
cv2.circle(side_mask, centerFt, radiusFt, np.array([0,0,255]), 10)
cv2.imshow('Red mask', side_mask)
else:
numberErrorsFt = numberErrorsFt + 1
def get_green_circle(frame, hlg, slg, vlg, hhg, shg, vhg):
global numberErrorsBk
global centerBk
global radiusBk
side_mask = make_image_mask_green(frame, hlg, slg, vlg, hhg, shg, vhg) #
if type(side_mask) != type(''):
contours, hierarchy = cv2.findContours(side_mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
if len(contours):
cnt = contours[0]
(x,y),radiusBk = cv2.minEnclosingCircle(cnt)
centerBk = (int(x),int(y))
radiusBk = int(radiusBk)
side_mask = cv2.cvtColor(side_mask, cv2.COLOR_GRAY2RGB)
cv2.circle(side_mask, centerBk, radiusBk, np.array([0,255,0]), 10)
cv2.imshow('green mask', side_mask)
#print 'center: ', center, 'radius: ', radius, ' found from side camera'
else:
print 'no contours found for green'
numberErrorsBk = numberErrorsBk + 1
else:
print 'green string'
def findFtFlagMarker(frame, hlr, slr, vlr, hhr, shr, vhr):
get_red_circle(frame, hlr, slr, vlr, hhr, shr, vhr)
centerPFt, radiusPFt = centerFt, radiusFt
if centerFt:
centerArrayA.append(centerFt[0])
centerArrayB.append(centerFt[1])
else:
centerArrayA.append(centerPFt[0])
centerArrayB.append(centerPFt[1])
def findBkFlagMarker(frame, hlg, slg, vlg, hhg, shg, vhg):
get_green_circle(frame, hlg, slg, vlg, hhg, shg, vhg)
centerPBk, radiusPBk = centerBk, radiusBk
if centerBk:
centerArrayX.append(centerBk[0])
centerArrayY.append(centerBk[1])
else:
'In P block. centerP is: ', centerPBk
centerArrayX.append(centerPBk[0])
centerArrayY.append(centerPBk[1])
def printSummaryStats():
'''Display sumary stats: '''
print 'SUMMARY: '
print 'Back flag: ', numberErrorsBk, ' errors out of ', frameNum
print ' ', 100*float(numberErrorsBk)/frameNum, ' % frames without a circle found'
print 'Front flag: ', numberErrorsFt, ' errors out of ', frameNum
print ' ', 100*float(numberErrorsFt)/frameNum, ' % frames without a circle found'
def plotBothFlagPositions():
##Plot data!:
plt.plot(centerArrayX, centerArrayY, marker='.', markersize=10, alpha=.7, linestyle='None', label = 'Back Flag Position', color='#7A9E35', linewidth=6)
plt.plot(centerArrayA, centerArrayB, marker='.', markersize=10, alpha=.7, linestyle='None', label = 'Front Flag Position', color='#AA3C39', linewidth=6)
plt.xlabel('X Position (pixels)', fontsize = 18)
plt.ylabel('Y Position (pixels)', fontsize = 18)
plt.title('Position of Fluttering Sail Through Time', fontsize = 20)
plt.axis('equal') #HALIE I ADDED THESE SO THAT IT WOULD BE EASIER TO SEE CIRCLES, DELETE IF YOU WANT
plt.legend()
plt.show()
def plotXXPositions():
plt.plot(centerArrayB, centerArrayY, marker='.', markersize=10, alpha=.7, linestyle='None', color='#7A9E35', linewidth=6)
plt.xlabel('Front Flag Y Position (pixels)', fontsize = 18)
plt.ylabel('Back Flag Y Position (pixels)', fontsize = 18)
plt.title('Position of Fluttering Sail Through Time', fontsize = 20)
#plt.legend()
plt.axis('equal') #HALIE I ADDED THESE SO THAT IT WOULD BE EASIER TO SEE CIRCLES, DELETE IF YOU WANT
plt.show()
##Open Video file:
cap = cv2.VideoCapture(filename)
while(cap.isOpened()):
ret, frame = cap.read()
height, width, channels = frame.shape
frame = frame[450:550, 150:width-300] # Crop from x, y, w, h -> 100, 200, 300, 400
cv2.imshow('Cropped image', frame)
#findFtFlagMarker(frame)
hlg = cv2.getTrackbarPos('H_low_green','greenHSVMask')
slg = cv2.getTrackbarPos('S_low_green','greenHSVMask')
vlg = cv2.getTrackbarPos('V_low_green','greenHSVMask')
hhg = cv2.getTrackbarPos('H_high_green','greenHSVMask')
shg = cv2.getTrackbarPos('S_high_green','greenHSVMask')
vhg = cv2.getTrackbarPos('V_high_green','greenHSVMask')
hlr = cv2.getTrackbarPos('H_low_red','redHSVMask')
slr = cv2.getTrackbarPos('S_low_red','redHSVMask')
vlr = cv2.getTrackbarPos('V_low_red','redHSVMask')
hhr = cv2.getTrackbarPos('H_high_red','redHSVMask')
shr = cv2.getTrackbarPos('S_high_red','redHSVMask')
vhr = cv2.getTrackbarPos('V_high_red','redHSVMask')
## Testing only, make image masks
#make_image_mask_green(frame, hlg, slg, vlg, hhg, shg, vhg)
#make_image_mask_red(frame, hlr, slr, vlr, hhr, shr, vhr)
## Testing only, only find circles
#get_red_circle(frame, hlr, slr, vlr, hhr, shr, vhr)
#get_green_circle(frame, hlg, slg, vlg, hhg, shg, vhg)
## Production Use!!!
findFtFlagMarker(frame, hlr, slr, vlr, hhr, shr, vhr)
findBkFlagMarker(frame, hlg, slg, vlg, hhg, shg, vhg)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
frameNum =frameNum + 1
if frameNum >= cap.get(cv2.cv.CV_CAP_PROP_FRAME_COUNT):
## Now that we're done, let's save the data!
pickle.dump(centerArrayX, open( "Xt"+str(testNum)+".p", "wb" ) )
pickle.dump(centerArrayY, open( "Yt"+str(testNum)+".p", "wb" ) )
pickle.dump(centerArrayA, open( "At"+str(testNum)+".p", "wb" ) )
pickle.dump(centerArrayB, open( "Bt"+str(testNum)+".p", "wb" ) )
## ##Filter out extreme values:
## stdx = stats.tstd(centerArrayX)
## meanx = stats.tmean(centerArrayX)
## for x in centerArrayX:
## if
# centerArrayX = np.array(centerArrayX)
# plt.plot(centerArrayX, label='2')
# centerArrayX = reject_outliers(centerArrayX)
# plt.plot(centerArrayX, label='3')
printSummaryStats()
plotBothFlagPositions()
plotXXPositions()
break
cap.release()
#cv2.destroyAllWindows()