105 lines
3.3 KiB
Python
105 lines
3.3 KiB
Python
#!/usr/bin/env python2
|
|
|
|
import cv2, math
|
|
import numpy as np
|
|
|
|
#capture from camera at location 0
|
|
cap = cv2.VideoCapture(0)
|
|
#set the width and height, and UNSUCCESSFULLY set the exposure time
|
|
cap.set(3,1280)
|
|
cap.set(4,1024)
|
|
cap.set(15, 0.1)
|
|
|
|
def dist(a,b):
|
|
return math.sqrt( (a[0] - b[0]) * (a[0] - b[0]) + (a[1] - b[1]) * (a[1] - b[1]) )
|
|
|
|
# Needs calibration
|
|
ideal_pos = (334,309)
|
|
# Needs calibration
|
|
ideal_r = (309-214)
|
|
|
|
while True:
|
|
|
|
best_circles = []
|
|
samples_n = 5
|
|
|
|
for c in range(samples_n):
|
|
|
|
ret, img = cap.read()
|
|
center = (img.shape[1] / 2, img.shape[0] / 2)
|
|
# cv2.line(img, (center[0],0), (center[0], img.shape[1]), (200,0,0))
|
|
# img = img[0:img.shape[0], 0:img.shape[1]/2]
|
|
img = cv2.medianBlur(img, 3)
|
|
# print("Captured image is %d x %d pixels, center is %d x %d" % (img.shape[1], img.shape[0], center[0], center[1]))
|
|
cv2.imshow("input", img)
|
|
#cv2.imshow("thresholded", imgray*thresh2)
|
|
edges = cv2.Canny(img,100,200)
|
|
|
|
circles = cv2.HoughCircles(edges,cv2.cv.CV_HOUGH_GRADIENT,1,20,param1=50,param2=30,minRadius=np.uint16(ideal_r*0.9),maxRadius=np.uint16(ideal_r*1.1))
|
|
# print(circles)
|
|
|
|
if circles is not None:
|
|
|
|
best_c_p = None
|
|
best_c_r = None
|
|
best_c_score = 1000
|
|
|
|
circles = np.uint16(np.around(circles))
|
|
for i in circles[0,:]:
|
|
if dist(center, (i[0], i[1])) + abs(i[2] - ideal_r) < best_c_score:
|
|
best_c_p = (i[0], i[1])
|
|
best_c_r = i[2]
|
|
best_c_dist = dist(center, best_c_p)
|
|
|
|
best_circles.append(
|
|
{
|
|
"p": best_c_p,
|
|
"r": best_c_r,
|
|
"score": best_c_score,
|
|
}
|
|
)
|
|
|
|
# circles = np.uint16(np.around(circles))
|
|
# for i in circles[0,:]:
|
|
# # draw the outer circle
|
|
# cv2.circle(edges,(i[0],i[1]),i[2],(100,0,0),2)
|
|
# # draw the center of the circle
|
|
# cv2.circle(edges,(i[0],i[1]),2,(200,0,0),3)
|
|
|
|
# # draw the outer circle
|
|
# cv2.circle(edges,tuple(best_c_p),best_c_r,(100,0,0),2)
|
|
# # draw the center of the circle
|
|
# cv2.circle(edges,tuple(best_c_p),2,(200,0,0),3)
|
|
|
|
# cv2.imshow("edges", edges)
|
|
|
|
avg_best_c_p = [0,0]
|
|
avg_best_c_r = 0
|
|
|
|
if len(best_circles) > 0:
|
|
for c in best_circles:
|
|
avg_best_c_p[0] = avg_best_c_p[0] + c["p"][0]
|
|
avg_best_c_p[1] = avg_best_c_p[1] + c["p"][1]
|
|
avg_best_c_r = avg_best_c_r + c["r"]
|
|
|
|
avg_best_c_p[0] = np.uint16(avg_best_c_p[0] / len(best_circles))
|
|
avg_best_c_p[1] = np.uint16(avg_best_c_p[1] / len(best_circles))
|
|
avg_best_c_r = np.uint16(avg_best_c_r / len(best_circles))
|
|
|
|
print("Found best avg circle (samples=%d) p=%s r=%s" % (len(best_circles), avg_best_c_p, avg_best_c_r))
|
|
# draw the outer circle
|
|
cv2.circle(edges,tuple(avg_best_c_p),avg_best_c_r,(100,0,0),2)
|
|
# draw the center of the circle
|
|
cv2.circle(edges,tuple(avg_best_c_p),2,(200,0,0),3)
|
|
|
|
cv2.imshow("edges", edges)
|
|
|
|
key = cv2.waitKey(10)
|
|
if key == 27:
|
|
break
|
|
|
|
|
|
cv2.destroyAllWindows()
|
|
cv2.VideoCapture(0).release()
|
|
|