To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit 6ce85493 authored by widmerl's avatar widmerl
Browse files

added detection of obviousely wrong measurements

parent d2dece9d
......@@ -32,7 +32,9 @@ G means coloured in Green
|---|
diameter
Usage:
q to quit
c to change color, then type "lower h +10" to the Terminal to increase the lower bound of the h value
Copyright 2019 Lars Widmer
......@@ -55,11 +57,13 @@ from operator import itemgetter
from pykalman import KalmanFilter
WIDTH = 600
WIDTH = 640
BLURR = 11
ACT_SIZE = 100
FOV = 70
CAMERA = 1
CAMERA = 0
greenLower = (50, 50, 40)
greenUpper = (80, 255, 255)
......@@ -77,6 +81,13 @@ lower = greenLower
def angle(p1, p2):
return (math.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)/WIDTH)*FOV
def big_diff(v1, v2, mult):
return v1/v2 < 1/mult or v1/v2 > mult
def similar_value(v1,v2,max_diff):
return max_diff > abs(v1-v2)
#initialize font
fontface = cv2.FONT_HERSHEY_SIMPLEX
fontscale = 0.5
......@@ -113,6 +124,14 @@ args = vars(ap.parse_args())
points = []
t_meas_past = []
t_meas_kalman = []
radius_avg = 0
phi_ud = 0
phi_lr = 0
l = 0
phi_ud_old = 0
phi_lr_old = 0
l_old = 0
fig, ax = plt.subplots()
......@@ -131,9 +150,45 @@ else:
time.sleep(0.2)
# keep looping
while True:
#allow changing of color-range in runtime
key = cv2.waitKey(1) & 0xFF
if key == ord("c"):
colorchange = input()
space = colorchange[0:5]
field = colorchange[6]
value_change = int(colorchange[7:])
if space == 'upper':
if field == 'h':
upper = (upper[0]+value_change, upper[1], upper[2])
elif field == 's':
upper = (upper[0], upper[1]+value_change, upper[2])
elif field == 'v':
upper = (upper[0], upper[1], upper[2]+value_change)
else:
print('options are h, s or v')
elif space == 'lower':
if field == 'h':
lower = (lower[0]+value_change, lower[1], lower[2])
elif field == 's':
lower = (lower[0], lower[1]+value_change, lower[2])
elif field == 'v':
lower = (lower[0], lower[1], lower[2]+value_change)
else:
print('options are h, s or v')
else:
print('usage: upper h +10')
print('new upper: ', upper)
print('new lower: ', lower)
#clear points queue
points = []
#mark measurements that are improbable (reset variable)
mark_missing = False
# grab the current frame
frame = vs.read()
......@@ -159,7 +214,7 @@ while True:
mask = cv2.inRange(hsv, lower, upper)
cv2.imshow("Masked", mask)
mask = cv2.erode(mask, None, iterations=2)
cv2.imshow("Eroded", mask)
#cv2.imshow("Eroded", mask)
mask = cv2.dilate(mask, None, iterations=2)
cv2.imshow("Dilated", mask)
......@@ -189,10 +244,12 @@ while True:
cv2.circle(frame, center, 5, (0, 0, 255), -1)
#add to points list
points.append((x,y))
points.append((x,y,radius))
radius_avg += radius
#calculate new Vector if all four points visible
if len(points) == 4:
radius_avg = radius_avg/4
points.sort(key = itemgetter(1))
down = sorted(points[-2:], key = itemgetter(0))
up = sorted(points[:2], key = itemgetter(0))
......@@ -201,40 +258,58 @@ while True:
down_left = down[0]
down_right = down[1]
cv2.putText(frame, 'up_left', (int(up_left[0]),int(up_left[1])), fontface, fontscale, fontcolor)
cv2.putText(frame, 'up_right', (int(up_right[0]),int(up_right[1])), fontface, fontscale, fontcolor)
cv2.putText(frame, 'down_left', (int(down_left[0]),int(down_left[1])), fontface, fontscale, fontcolor)
cv2.putText(frame, 'down_right', (int(down_right[0]),int(down_right[1])), fontface, fontscale, fontcolor)
t_meas = angle(up_left, up_right)
b_meas = angle(down_left, down_right)
l_meas = angle(up_left, down_left)
r_meas = angle(up_right, down_right)
t_meas_past.append(t_meas)
#mark measurements that are improbable
for point in points:
if big_diff(point[2], radius_avg, 2):
mark_missing = True
l_t = ACT_SIZE/math.sin(t_meas*0.01745)
l_b = ACT_SIZE/math.sin(b_meas*0.01745)
l_l = ACT_SIZE/math.sin(l_meas*0.01745)
l_r = ACT_SIZE/math.sin(r_meas*0.01745)
if not mark_missing:
l = (l_t+l_b+l_r+l_l)/4
phi_ud = math.atan((l_t - l_b)/ACT_SIZE)
phi_lr = math.atan((l_l - l_r)/ACT_SIZE)
output = 'dist: ' + str(int(l)) + ' phi_ud: ' + "{:5.1f}".format(phi_ud * 57.296) + ' phi_lr: ' + "{:5.1f}".format(phi_lr * 57.296)
cv2.putText(frame, output, (0,20), fontface, fontscale, fontcolor)
cv2.putText(frame, 'up_left', (int(up_left[0]),int(up_left[1])), fontface, fontscale, fontcolor)
cv2.putText(frame, 'up_right', (int(up_right[0]),int(up_right[1])), fontface, fontscale, fontcolor)
cv2.putText(frame, 'down_left', (int(down_left[0]),int(down_left[1])), fontface, fontscale, fontcolor)
cv2.putText(frame, 'down_right', (int(down_right[0]),int(down_right[1])), fontface, fontscale, fontcolor)
t_meas = angle(up_left, up_right)
b_meas = angle(down_left, down_right)
l_meas = angle(up_left, down_left)
r_meas = angle(up_right, down_right)
t_meas_past.append(t_meas)
l_t = ACT_SIZE/math.sin(t_meas*0.01745)
l_b = ACT_SIZE/math.sin(b_meas*0.01745)
l_l = ACT_SIZE/math.sin(l_meas*0.01745)
l_r = ACT_SIZE/math.sin(r_meas*0.01745)
l_old = l
phi_ud_old = phi_ud
phi_lr_old = phi_lr
l = (l_t+l_b+l_r+l_l)/4
phi_ud = math.atan((l_t - l_b)/ACT_SIZE)
phi_lr = math.atan((l_l - l_r)/ACT_SIZE)
if similar_value(l,l_old,10) or similar_value(phi_ud,phi_ud_old,5) or similar_value(phi_lr,phi_lr_old,5):
output = 'dist: ' + str(int(l)) + ' phi_ud: ' + "{:5.1f}".format(phi_ud * 57.296) + ' phi_lr: ' + "{:5.1f}".format(phi_lr * 57.296)
else:
mark_missing = True
cv2.putText(frame, output, (0,20), fontface, fontscale, fontcolor)
# t_meas_kalman.append( #???????????????????????????????????)
else:
mark_missing = True
if mark_missing:
cv2.putText(frame, 'No marker detected', (0,20), fontface, fontscale, fontcolor)
# show the frame to our screen
cv2.imshow("Frame", frame)
#print(points)
# cv2.imshow("Frame", frame)
key = cv2.waitKey(1) & 0xFF
# if the 'q' key is pressed, stop the loop
if key == ord("q"):
......@@ -248,9 +323,9 @@ if not args.get("video", False):
else:
vs.release()
# close all windows
plt.plot(t_meas_past)
plt.ylabel('t_measured')
plt.show()
#plt.plot(t_meas_past)
#plt.ylabel('t_measured')
#plt.show()
time.sleep(0.1)
while True:
if key == ord("q"):
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment