def main():
DIM = (640, 480)
K = np.array([[333.71994064649397, 0.0, 328.90247793390785],
[0.0, 332.3864141021737, 221.06175468227764],
[0.0, 0.0, 1.0]])
D = np.array([[-0.022627097023142008], [-0.045919790866641955],
[0.055605531135859636], [-0.02114945903451172]])
video_capture = cv2.VideoCapture(0)
map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), K, DIM,
cv2.CV_16SC2)
detector = apriltag.Detector(searchpath=apriltag._get_demo_searchpath())
while (True):
try:
_, frame = video_capture.read()
undistorted_img = cv2.remap(frame,
map1,
map2,
interpolation=cv2.INTER_LINEAR,
borderMode=cv2.BORDER_CONSTANT)
gray_frame = cv2.cvtColor(undistorted_img, cv2.COLOR_BGR2GRAY)
result = detector.detect(gray_frame)
if result:
frame, angle = draw_tag(undistorted_img, result)
print(angle)
cv2.imshow("april_tags", undistorted_img)
cv2.waitKey(1)
except KeyboardInterrupt:
break
video_capture.release()
def main():
parser = ArgumentParser(description='test apriltag Python bindings')
parser.add_argument('device_or_movie', metavar='INPUT', nargs='?', default=0,help='Movie to load or integer ID of camera device')
apriltag.add_arguments(parser)
options = parser.parse_args()
webcam = cv.VideoCapture(0)
cv.namedWindow('frame')
detector = apriltag.Detector(options,searchpath=apriltag._get_demo_searchpath())
while True:
_,frame = webcam.read()
gray = cv.cvtColor(frame,cv.COLOR_RGB2GRAY)
detections, dimg = detector.detect(gray, return_image=True)
num_detections = len(detections)
print('Detected {} tags.\n'.format(num_detections))
try:
print (detections[0])
except:
print ("not found")
overlay = frame // 2 + dimg[:, :, None] // 2
cv.imshow('frame', overlay)
ikey = cv.waitKey(1)
if ikey == ord('q'):
break
if __name__ == '__main__':
main()
def main():
DIM = (640, 480)
camera_params = (506.66588415210174, 507.57637424966526,
311.03765199523536, 238.60300515336095)
tag_size = 160
video_capture = cv2.VideoCapture(0)
detector = apriltag.Detector(searchpath=apriltag._get_demo_searchpath())
while (True):
try:
_, frame = video_capture.read()
gray_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
result = detector.detect(gray_frame)
'''
if (result):
frame, angle = draw_tag(frame,result)
print(angle)
'''
for i, detection in enumerate(result):
pose, e0, e1 = detector.detection_pose(detection,
camera_params, tag_size)
draw_pose(frame, camera_params, tag_size, pose)
angles = rotationMatrixToEulerAngles(pose[0:3, 0:3])
print(angles, pose[0:3, 3])
cv2.imshow("apriltags", frame)
cv2.waitKey(1)
except KeyboardInterrupt:
break
video_capture.release()
def main():
args = get_args()
cols = args["cols"]
rows = args["rows"]
camera = util.get_camera(args["index"])
detector = apriltag.Detector(searchpath=apriltag._get_demo_searchpath())
start = time.time()
frame_num = 0
window = cv2.namedWindow("frame")
while True:
if PRINT_FPS:
frame_num += 1
ret, frame = camera.read()
if SHOW_IMAGE:
cv2.imshow("frame", frame)
if PRINT_FPS:
fps = frame_num / (time.time() - start)
print("Showing frame {}".format(frame_num))
print("Current speed: {} fps".format(fps))
if APPLY_CHECKERBOARD:
frame = undistort_image(frame, args["file"])
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
detections, det_image = detector.detect(gray, return_image=True)
print(len(detections))
if detections and APPLY_TRANSFORM:
detections = apply_transform(detections, args["file"])
points = []
dim_lst = []
for d in range(len(detections)):
tag_x, tag_y = detections[d].center
center_point = (int(tag_x), int(tag_y))
points.append(center_point)
cp = get_point(detections[d].center)
tl = get_point(detections[d].corners[0])
tr = get_point(detections[d].corners[1])
br = get_point(detections[d].corners[2])
x_axis = (tl[0] + tr[0]) // 2, (tl[1] + tr[1]) // 2
y_axis = (tr[0] + br[0]) // 2, (tr[1] + br[1]) // 2
# print("Tag {} found at ({},{})".format(d.tag_id, tag_x, tag_y))
# print("with angle {}".format(util.get_tag_angle(d.corners)))
# Compute size of each tag as they appear on camera
tl = get_point(detections[d].corners[0])
tr = get_point(detections[d].corners[1])
br = get_point(detections[d].corners[2])
width = dist(tr, tl)
height = dist(tr, br)
dim = np.array([width, height])
dim_lst.append(dim)
compute_dx_dy_all(frame, detections, points, dim_lst, rows, cols,
CANONICAL_TAG)
if cv2.waitKey(1) & 0xFF == ord("q"):
break
camera.release()
cv2.destroyAllWindows()
pass
def main():
'''Main function.'''
parser = ArgumentParser(description='test apriltag Python bindings')
parser.add_argument('device_or_movie',
metavar='INPUT',
nargs='?',
default=0,
help='Movie to load or integer ID of camera device')
apriltag.add_arguments(parser)
options = parser.parse_args()
try:
cap = cv2.VideoCapture(int(options.device_or_movie))
except ValueError:
cap = cv2.VideoCapture(options.device_or_movie)
window = 'Camera'
cv2.namedWindow(window)
# set up a reasonable search path for the apriltag DLL inside the
# github repo this file lives in;
#
# for "real" deployments, either install the DLL in the appropriate
# system-wide library directory, or specify your own search paths
# as needed.
detector = apriltag.Detector(options,
searchpath=apriltag._get_demo_searchpath())
while True:
success, frame = cap.read()
if not success:
break
gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
detections, dimg = detector.detect(gray, return_image=True)
num_detections = len(detections)
print('Detected {} tags.\n'.format(num_detections))
for i, detection in enumerate(detections):
print('Detection {} of {}:'.format(i + 1, num_detections))
print()
print(detection.tostring(indent=2))
print()
overlay = frame // 2 + dimg[:, :, None] // 2
cv2.imshow(window, overlay)
k = cv2.waitKey(1)
if k == 27:
break
def main():
DIM = (640, 480)
camera_params = (506.66588415210174, 507.57637424966526,
311.03765199523536, 238.60300515336095)
tag_size = 160
video_capture = cv2.VideoCapture(0)
detector = apriltag.Detector(searchpath=apriltag._get_demo_searchpath())
# ROS Publisher Init
pub = rospy.Publisher('tagPose', tagInfoArray, queue_size=5)
rospy.init_node('talker', anonymous=True)
# Init array of tag poses
#tagPoses = tagInfoArray()
while (True):
_, frame = video_capture.read()
gray_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
result = detector.detect(gray_frame)
# Init fresh tag arr to pass
tagPoses = tagInfoArray()
'''
if (result):
frame, angle = draw_tag(frame,result)
print(angle)
'''
for i, detection in enumerate(result):
pose, e0, e1 = detector.detection_pose(detection, camera_params,
tag_size)
draw_pose(frame, camera_params, tag_size, pose)
#print(result[i].tag_id)
tagPoses.tags.append(tagDataToPose(pose, result[i].tag_id))
# print(angles,pose[0:3,3])
print(tagPoses)
pub.publish(tagPoses)
cv2.imshow("apriltags", frame)
k = cv2.waitKey(1)
if (k % 256 == 32):
break
"""
except KeyboardInterrupt:
break
"""
video_capture.release()
def main():
video_capture = cv2.VideoCapture(0)
#video_capture.set(3, 640)
#video_capture.set(4, 480)
detector = apriltag.Detector(searchpath=apriltag._get_demo_searchpath())
while (True):
try:
_, frame = video_capture.read()
gray_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
result = detector.detect(gray_frame)
if result:
frame, angle = draw_tag(frame, result)
print(angle)
cv2.imshow("apriltags", frame)
cv2.waitKey(1)
except KeyboardInterrupt:
break
video_capture.release
def __init__(self, markers):
self.options = apriltag.DetectorOptions(families='tag36h11',
border=1,
nthreads=4,
quad_decimate=1.0,
quad_blur=0.0,
refine_edges=True,
refine_decode=False,
refine_pose=False,
debug=False,
quad_contours=True)
self.detector = apriltag.Detector(self.options)
self.detector = apriltag.Detector(
self.options, searchpath=apriltag._get_demo_searchpath())
self.kalman_filters = {}
self.transformation_ka = Kalman(12)
self.g_camera_base = None
self.marker_pose_dict = {}
self.markers = markers
self.dimg = None # apriltage detection overlay matrix
self.kalman_count = {}
for tid in self.markers:
self.kalman_count[tid] = 0
def analyze(lock, options, pre_detection, save_time, bee_time, pre, cnt,
pre_num_name, save_prefix, bee_log_dict, start_time):
lock[0] = 'locked'
fh_id_log = open(save_prefix + 'ImgID.log', 'a')
#record time for each picuture
window = 'Camera'
cv2.namedWindow(window)
detector = apriltag.Detector(options,
searchpath=apriltag._get_demo_searchpath())
rgb = cv2.imread(save_prefix + " top" + str(cnt) + ".jpg")
print(save_prefix + "top" + str(cnt) + ".jpg")
while rgb is None:
rgb = cv2.imread(save_prefix + "top" + str(cnt) + ".jpg")
t.sleep(0.1)
gray = cv2.cvtColor(rgb, cv2.COLOR_RGB2GRAY)
k = t.time()
detections, dimg = detector.detect(gray, return_image=True)
print('detecting time:', t.time() - k)
lock[0] = 'unlocked'
num_detections = len(detections)
print('Detected {} tags.\n'.format(num_detections))
current_detection = []
overlay = rgb // 2 + dimg[:, :, None] // 2
if len(detections):
for i, detection in enumerate(detections):
centerY = detection.center[1]
#assume bee only travels one direction a time
#if the tag first time appears, start append to the list until the tag dispears
current_detection.append(detection[1])
print('previous', pre_detection)
if detection[1] in pre_detection.keys():
if (float(centerY) - float(pre_detection[detection[1]]) < 0):
fh_id_log.write(
str(cnt) + '\t' + str(detection[1]) + ' exit ' +
str(bee_time[cnt]) + '\n')
#add_bee_event(pre,bee_log_dict,tag[0],t.time()-start_time,True)
else:
fh_id_log.write(
str(cnt) + '\t' + str(detection[1]) + ' enter ' +
str(bee_time[cnt]) + '\n')
#add_bee_event(pre,bee_log_dict,tag[0],t.time()-start_time,False)
else:
fh_id_log.write(
str(cnt) + '\t' + str(detection[1]) + ' first ' +
str(bee_time[cnt]) + '\n')
font = cv2.FONT_HERSHEY_SIMPLEX
bottomLeftCornerOfText = (400, int(centerY))
fontScale = 1
fontColor = (0, 255, 255)
lineType = 2
cv2.putText(rgb, str(detection[1]), bottomLeftCornerOfText, font,
fontScale, fontColor, lineType)
for i, detection in enumerate(detections):
pre_detection[detection[1]] = detection.center[
1] #find the previous tags
else:
fh_id_log.write(str(cnt) + '\t' + '-1' + '\n')
pre_detection.clear()
fh_id_log.close()
cv2.imwrite(save_prefix + " top" + str(cnt) + ".jpg", rgb)
os.system('sync')
def main():
camera = setup_camera()
detector = apriltag.Detector(searchpath=apriltag._get_demo_searchpath())
start = time.time()
frame_num = 0
while True:
frame_num += 1
ret, frame = camera.read()
if not DETECT_TAGS:
if SHOW_IMAGE:
cv2.imshow("frame", frame)
fps = frame_num / (time.time() - start)
# print("Showing frame {}".format(frame_num))
# print("Current speed: {} fps".format(fps))
if DETECT_TAGS:
horizontal_dist = []
vertical_dist = []
row = -1
gray = cvtColor(frame, cv2.COLOR_BGR2GRAY)
detections, det_image = detector.detect(gray, return_image=True)
print(len(detections))
points = []
for d in range(len(detections)):
tag_x, tag_y = detections[d].center
center_point = (int(tag_x), int(tag_y))
points.append(center_point)
cp = get_point(detections[d].center)
tl = get_point(detections[d].corners[0])
tr = get_point(detections[d].corners[1])
br = get_point(detections[d].corners[2])
x_axis = (tl[0] + tr[0]) // 2, (tl[1] + tr[1]) // 2
y_axis = (tr[0] + br[0]) // 2, (tr[1] + br[1]) // 2
# print("Tag {} found at ({},{})".format(d.tag_id, tag_x, tag_y))
# print("with angle {}".format(util.get_tag_angle(d.corners)))
# 2D array to keep track of horizontal distance of current tag
# and the one next to it
if detections[d].center[0] < detections[d - 1].center[0]:
print("adding new array")
horizontal_dist.append([])
row += 1
else:
print("current_row")
print(row)
horizontal_dist[row].append(detections[d].center[0] -
detections[d - 1].center[0])
cv2.circle(frame, (int(tag_x), int(tag_y)), 5, (0, 0, 255))
cv2.line(frame, cp, x_axis, (0, 0, 255), 5)
cv2.line(frame, cp, y_axis, (0, 255, 0), 5)
if SHOW_IMAGE:
cv2.imshow("frame", frame)
# 2D array to keep track of vertical distance of current tag and the
# one immediate below it.
num_cols = len(horizontal_dist[0]) + 1
for i in range(num_cols):
vertical_dist.append([])
for j in range(row):
vertical_dist[i][j] = (detections[i + num_cols].center[1] -
detections[i].center[1])
horizontal_dist = np.array(horizontal_dist)
vertical_dist = np.array(vertical_dist)
horizontal_median = np.median(horizontal_dist)
horizontal_std = np.std(horizontal_dist)
vertical_median = np.median(vertical_dist)
vertical_std = np.std(vertical_dist)
x_pts, y_pts = [p[0] for p in points], [p[1] for p in points]
dx, dy = [], []
for col in range(8 - 1):
for row in range(3):
dx.append(x_pts[8 * row + col + 1] - x_pts[8 * row + col])
dy.append(y_pts[8 * row + col + 1] - y_pts[8 * row + col])
print("H: {}".format(dx))
print("V: {}".format(dy))
print("H: {}".format(horizontal_dist))
print("H median: {}".format(horizontal_median))
print("H stddev: {}".format(horizontal_std))
print("")
print("V: {}".format(vertical_dist))
print("V median: {}".format(vertical_median))
print("V stddev: {}".format(vertical_std))
print("")
if cv2.waitKey(1) & 0xFF == ord("q"):
break
camera.release()
cv2.destroyAllWindows()
pass
import RPi.GPIO as rp
from time import sleep
from argparse import ArgumentParser
import apriltag
from Methods import *
import threading
##############################
def map( x, in_min,in_max, out_min, out_max):
maps = -((x - in_max)*(out_max-out_min)/(in_max-in_min)+out_min)
return maps
##############################
parser = ArgumentParser(description='test apriltag Python bindings')
parser.add_argument('device_or_movie', metavar='INPUT', nargs='?', default=0,help='Movie to load or integer ID of camera device')
apriltag.add_arguments(parser)
options = parser.parse_args()
detector = apriltag.Detector(options,searchpath=apriltag._get_demo_searchpath())
##############################
kti = 10
ksa = 10
servo1 = 15
servo2 = 14
deltadt = 0.1
dt1 = 4
dt2 = 6
cv.namedWindow('window')
tagid = 7
thread1 = threading.Thread(target=play_action,args=('Soccer_Forward'))
##############################
rp.setmode(rp.BCM)
rp.setwarnings(False)
rp.setup(servo1,rp.OUT)
def main():
'''Main function.'''
parser = ArgumentParser(
description='test apriltag Python bindings')
# parser.add_argument('device_or_movie', metavar='INPUT', nargs='?', default=0,
# help='Movie to load or integer ID of camera device')
parser.add_argument('-k', '--camera-params', type=_camera_params,
default=None,
help='intrinsic parameters for camera (in the form fx,fy,cx,cy)')
parser.add_argument('-s', '--tag-size', type=float,
default=1.0,
help='tag size in user-specified units (default=1.0)')
add_arguments(parser)
options = parser.parse_args()
apriltag.add_arguments(parser)
options = parser.parse_args()
# try:
# cap = cv2.VideoCapture(int(options.device_or_movie))
# except ValueError:
# cap = cv2.VideoCapture(options.device_or_movie)
cap = cv2.VideoCapture(1)
window = 'Camera'
cv2.namedWindow(window)
# set up a reasonable search path for the apriltag DLL inside the
# github repo this file lives in;
#
# for "real" deployments, either install the DLL in the appropriate
# system-wide library directory, or specify your own search paths
# as needed.
detector = apriltag.Detector(options,
searchpath=apriltag._get_demo_searchpath())
while True:
success, frame = cap.read()
if not success:
break
gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
detections, dimg = detector.detect(gray, return_image=True)
num_detections = len(detections)
print('Detected {} tags.\n'.format(num_detections))
for i, detection in enumerate(detections):
print( 'Detection {} of {}:'.format(i+1, num_detections))
print()
print(detection.tostring(indent=2))
#if options.camera_params is not None:
pose, e0, e1 = detector.detection_pose(detection,
options.camera_params,
options.tag_size)
if _HAVE_CV2:
_draw_pose(overlay,
options.camera_params,
options.tag_size,
pose)
print(detection.tostring(
collections.OrderedDict([('Pose',pose),
('InitError', e0),
('FinalError', e1)]),
indent=2))
print()
overlay = frame // 2 + dimg[:, :, None] // 2
cv2.imshow(window, overlay)
k = cv2.waitKey(1)
if k == 27:
break
####################################
cv.createTrackbar('smt1', 'window', 0, 10, nothing)
cv.createTrackbar('smt2', 'window', 0, 10, nothing)
cv.createTrackbar('smt3', 'window', 0, 10, nothing)
cv.createTrackbar('smt4', 'window', 0, 10, nothing)
###/################################
parser = ArgumentParser(description='test apriltag Python bindings')
parser.add_argument('device_or_movie',
metavar='INPUT',
nargs='?',
default=0,
help='Movie to load or integer ID of camera device')
apriltag.add_arguments(parser)
options = parser.parse_args()
detector = apriltag.Detector(options,
searchpath=apriltag._get_demo_searchpath())
###/################################
while True:
_, frame = webcam.read()
mrkzy = int(len(frame) / 2)
mrkzx = int(len(frame[0]) / 2)
xframe = len(frame[0])
yframe = len(frame)
smt1 = cv.getTrackbarPos('smt1', 'window')
smt2 = cv.getTrackbarPos('smt2', 'window')
smt3 = cv.getTrackbarPos('smt3', 'window')
smt4 = cv.getTrackbarPos('smt4', 'window')
gray = cv.cvtColor(frame, cv.COLOR_RGB2GRAY)
detections, dimg = detector.detect(gray, return_image=True)
hilight = frame // 2 + dimg[:, :, None] // 2
def locate_tag(camera, data):
camera_matrix = np.array(data["camera_matrix"])
dist_coeffs = np.array(data["dist_coeffs"])
new_camera_matrix = np.array(data["new_camera_matrix"])
transformation_matrix = np.array(data["transformation_matrix"])
detector = apriltag.Detector(
searchpath=apriltag._get_demo_searchpath())
frame = []
gray = []
while True:
if not camera.isOpened():
print("Failed to open camera")
exit(0)
ret, frame = camera.read()
cv2.imshow("april_tag", frame)
# take a picture and get detections
k = cv2.waitKey(1)
if k % 256 == 32:
print("space has been pressed")
frame = get_image(camera)
gray = cvtColor(frame, COLOR_BGR2GRAY)
cv2.imshow("april_tag", frame)
# dst = cv2.undistort(frame, camera_matrix,
# dist_coeffs, None, new_camera_matrix)
# gray = cvtColor(dst, COLOR_BGR2GRAY)
detections, det_image = detector.detect(gray,
return_image=True)
if len(detections) == 0:
print("no tag found")
continue
print("Found " + str(len(detections)) + " apriltags")
for d in detections:
tag_x, tag_y = d.center
top_left = d.corners[0]
top_right = d.corners[1]
bottom_right = d.corners[2]
bottom_left = d.corners[3]
# TODO draw tag - might be better to generalize, because
# locate_cameras does this too.
# tag_x, tag_y = d.center
# print("Tag {} found at ({},{})".format(d.tag_id, tag_x, tag_y))
# # cv2.circle(frame, d.center, 5, BLUE)
# cv2.circle(frame, (int(tag_x), int(tag_y)), 5, (0, 0, 255))
# (x, y, z, angle) = compute_tag_undistorted_pose(
# camera_matrix, dist_coeffs, transformation_matrix, d, TAG_SIZE)
# Scale the coordinates, and print for debugging
# prints Device ID :: tag id :: x y z angle
# TODO debug offset method - is better, but not perfect.
# x = MULT_FACTOR * (x)
# y = MULT_FACTOR * (y)
# print(tag_xyz)
print("Tag {} found at ({},{})".format(
d.tag_id, tag_x, tag_y))
cv2.circle(frame, (int(tag_x), int(tag_y)), 5, (0, 0, 255))
# angle = math.radians(360 - angle)
# cv2.line(
# frame, (int(x + 800), int(y)), (int(x + 800 + 50 *
# math.cos(angle)), int(y + 50*math.sin(angle))), (255, 0, 0), 5)
# cv2.line(
# frame, (int(x + 800), int(y)), (int(x))
# )
cv2.imshow("april_tag", frame)
if k % 256 == 27:
break
def __init__(self):
self.detector = None
if 'apriltag' in sys.modules:
self.detector = apriltag.Detector(
searchpath=apriltag._get_demo_searchpath())
def main():
transmitCntr=0
hostname = socket.gethostname()
ip_address = socket.gethostbyname(hostname)
HOST = '192.168.1.78' # The server's hostname or IP address # This is the server
PORT = 65432 # The port used by the server
print(ip_address)
serverSocket=socket.socket(socket.AF_INET,socket.SOCK_STREAM)
serverSocket.bind((HOST,PORT))
serverSocket.listen()
robotClient,addr=serverSocket.accept()
msg=robotClient.recv(1024).decode('ascii')
if(msg=='RobotConnected'):
print('Starting Localization for 1 robot')
parser = ArgumentParser(
description='test apriltag Python bindings')
parser.add_argument('device_or_movie', metavar='INPUT', nargs='?', default=0,
help='Movie to load or integer ID of camera device')
orient = 0
apriltag.add_arguments(parser)
options = parser.parse_args()
try:
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 2560)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 1440)
except ValueError:
cap = cv2.VideoCapture(options.device_or_movie)
window = 'Camera'
window2 = 'Overlay1'
cv2.namedWindow(window)
cv2.namedWindow(window2)
detector = apriltag.Detector(options,
searchpath=apriltag._get_demo_searchpath())
font = cv2.FONT_HERSHEY_SIMPLEX
fontScale = 0.3
fontColor = (0,0,255)
lineType = 1
print(type(fontColor))
while True:
endpt=[0,0]
strtPt=[0,0]
strtFlag=False
endptFlag=False
success, frame = cap.read()
if not success:
break
gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
detections, dimg = detector.detect(gray, return_image=True)
headDir=np.array([0,0])
num_detections = len(detections)
#print('Detected {} tags.\n'.format(num_detections))
#dimg=np.zeros
dimg1=dimg
for i, detection in enumerate(detections):
dimg1 = draw(frame,detection.corners)
center=detection.center
centerTxt=((center.ravel()).astype(int)).astype(str)
if detection.tag_id==6:
strtPt=center
strtFlag=True
headDir=headingDir(detection.corners,center)
if detection.tag_id==9:
dimg1=cv2.putText(dimg1,'End Point', tuple((center.ravel()).astype(int)),font,0.8,fontColor,2)
dimg1 = cv2.circle(dimg1, tuple((center.ravel()).astype(int)),30, (0,128,255), 2)
endpt=center
endptFlag=True
else:
cv2.putText(dimg1,'Id:'+str(detection.tag_id), tuple((center.ravel()).astype(int)),font,0.8,(0,0,0),2)
botDir=headingDir(detection.corners,center)
dimg1=draw1(dimg1,botDir,center,(0,0,255))
cv2.putText(dimg1,'('+centerTxt[0]+','+centerTxt[1]+')', tuple((center.ravel()).astype(int)+10),font,fontScale,(255,0,0),lineType)
if num_detections >0:
if endptFlag and strtFlag:
orient=getTheta(strtPt,endpt,strtPt,headDir)
dimg1=cv2.putText(dimg1,'T:'+str(int(orient)), tuple((strtPt.ravel()).astype(int)+50),font,0.8,(0,0,0),2)
dimg1=draw1ine(dimg1,strtPt,endpt,(255,0,255))
overlay=dimg1
else:
overlay = frame
# Change the following line to get back the connection part.
transmitCntr=0
sndString=str(strtPt[0])+' '+str(strtPt[1])+' '+str(headDir[0])+' '+str(headDir[1])+' '+str(endptFlag)+' '+str(endpt[0])+' '+str(endpt[1])
if robotClient.recv(2048).decode('ascii')=='ok':
robotClient.send(CalTheta(sndString).encode('ascii'))
cv2.imshow(window, overlay)
def tracker(qlist, ids, goals, loc, obs, ob_ids, debug=False):
parser = ArgumentParser(description='test apriltag Python')
parser.add_argument('device_or_movie',
metavar='INPUT',
nargs='?',
default=1)
apriltag.add_arguments(parser)
options = parser.parse_args()
cap = cv2.VideoCapture(2)
# window = 'Camera'
win2 = "bigger_image"
cv2.namedWindow(win2)
# cv2.namedWindow(window)
dic = {}
dic2 = {}
detector = apriltag.Detector(options,
searchpath=apriltag._get_demo_searchpath())
got_detect = {}
got_detect2 = {}
while True:
# print("visual man loc", loc)
success, frame = cap.read()
if not success:
break
gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
r, c = gray.shape
detections, dimg = detector.detect(gray, return_image=True)
num_detections = len(detections)
if debug:
print(c, r)
print('Detected {} tags.\n'.format(num_detections))
for i in ids:
got_detect[i] = False
for i, detection in enumerate(detections):
# print('Detection {} of {}:'.format(i+1, num_detections))
# print()
# print(detection.tostring(indent=2))
# print(pose)
p1 = detection.corners[0]
p2 = detection.corners[1]
location = detection.center
tid = detection.tag_id
# print("detection_id is", tid)
got_detect[tid] = True
orient = orientation(p1, p2, r, c)
p = detection.center
x, y = p[0], p[1]
x, y = change_grid((x, y), r, c)
x, y = x * scale_x, y * scale_y
if debug:
print(p1, p2)
print(detection.corners)
print("center is", p)
print("x and y", x, y, "orientation:",
degrees(orientation(p1, p2, r, c)), "tid is", tid)
if tid in ids:
# print("found id",tid)
dic[tid] = (x, y, orient, 1)
# loc.value[tid] = (x, y, orient,1)
# print("x and y and orient", x,y,orient)
got_detect[tid] = True
# print("loc.value", loc.value, tid)
if debug:
print("stored in", tid)
if tid in ob_ids:
# print("**************************")
# print("Found obstacle {}".format(tid))
# print("**************************")
p = detection.center
x, y = p[0], p[1]
x, y = change_grid((x, y), r, c)
x, y = x * scale_x, y * scale_y
dic2[tid] = (x, y)
got_detect2[tid] = True
# print(theta_x, theta_y,theta_z)
for i in ids:
if not got_detect[i]:
dic[i] = (goals[i][0], goals[i][1], 0, 0)
# loc.value[i] = ((goals[i][0],goals[i][1],0))
default = {
12: (200, -210),
19: (
-320,
10,
)
}
for i in ob_ids:
if not got_detect2[i]:
dic2[i] = default[i]
# loc.value[i] = ((goals[i][0],goals[i][1],0))
loc.value = dic
obs.value = dic2
# print("loc.value", loc.value)
overlay = frame // 2 + dimg[:, :, None] // 2
# cv2.imshow(window, overlay)
scale_percent_r = 160
scale_percent_c = 150
width = int(c * scale_percent_c / 100)
height = int(r * scale_percent_r / 100)
dim = (width, height)
resized = cv2.resize(frame, dim, interpolation=cv2.INTER_AREA)
cv2.imshow(win2, resized)
k = cv2.waitKey(1)
if k == 27:
break
def main():
args = get_args()
BOARD_TAG_SIZE = args["board"]
ORIGIN_TAG_SIZE = args["origin"]
calib_file_name = args["file"]
# offsets to reposition where (0,0) is
x_offset = 0
y_offset = 0
camera = VideoCapture(0) # Open the camera and set camera params
if not VideoCapture.isOpened(camera):
print("Failed to open video capture device")
exit(0)
camera.set(CAP_PROP_FRAME_WIDTH, 1280)
camera.set(CAP_PROP_FRAME_HEIGHT, 720)
camera.set(CAP_PROP_FPS, 30)
# Get matrices from file
calib_file, camera_matrix, dist_coeffs = get_matrices_from_file(
calib_file_name)
calib_file.close()
print("Read from calibration file")
print("CAMERA MATRIX: {}".format(camera_matrix))
print("DIST COEFFS: {}".format(dist_coeffs))
print()
# Initialize the detector
detector = apriltag.Detector(searchpath=apriltag._get_demo_searchpath())
frame = []
gray = []
img_points = np.ndarray((4 * NUM_DETECTIONS, 2))
obj_points = np.ndarray((4 * NUM_DETECTIONS, 3))
detections = []
print(
"The program will now attempt to detect the 4 tags on the axis calibration board"
)
print(
"The four tags should have a red circle on their centers if detected properly."
)
print(
"There will also be a blue circle in the middle of the 4 tags if 4 are detected."
)
print("Align the blue dot with the middle of the screen.")
print("Then, press SPACE.")
while True:
frame = get_image(camera) # take a new picture
# For weird reasons, anti-distortion measures WORSENED the problem,
# so they have been removed. If you need to put them back,
# this is the place for it.
# Convert undistorted image to grayscale
gray = cvtColor(frame, COLOR_BGR2GRAY)
# Use the detector and compute useful values from it
detections, det_image = detector.detect(gray, return_image=True)
x_offset = 0
y_offset = 0
for i in range(len(detections)):
d = detections[i]
id = int(d.tag_id)
# Add to offsets
(ctr_x, ctr_y) = d.center
x_offset += ctr_x
y_offset += ctr_y
# Draw onto the frame
cv2.circle(frame, (int(ctr_x), int(ctr_y)), 5, (0, 0, 255), 3)
# Draw origin
if len(detections) == 4:
cv2.circle(frame, (int(x_offset / 4), int(y_offset / 4)), 5,
(255, 0, 0), 3)
imshow("Calibration board", frame)
if cv2.waitKey(1) & 0xFF == ord(" "):
break
else:
continue
cv2.destroyAllWindows()
# Compute transformation via PnP
# TODO What's the reasoning from this math?
# This was from a tutorial somehwhere and was directly
# transcribed from the C++ system.
for d in detections:
id = int(d.tag_id)
img_points[0 + 4 * id] = d.corners[0]
img_points[1 + 4 * id] = d.corners[1]
img_points[2 + 4 * id] = d.corners[2]
img_points[3 + 4 * id] = d.corners[3]
a = (id % 2) * 2 + 1
b = -((id / 2) * 2 - 1)
# 8.5 and 11 are letter paper dimensions!
x1 = -0.5 * BOARD_TAG_SIZE + a * 8.5 * 0.5
x2 = 0.5 * BOARD_TAG_SIZE + a * 8.5 * 0.5
y1 = -0.5 * BOARD_TAG_SIZE + b * 11 * 0.5
y2 = 0.5 * BOARD_TAG_SIZE + b * 11 * 0.5
obj_points[0 + 4 * id] = (x1, y1, 0.0)
obj_points[1 + 4 * id] = (x2, y1, 0.0)
obj_points[2 + 4 * id] = (x2, y2, 0.0)
obj_points[3 + 4 * id] = (x1, y2, 0.0)
# Make transform matrices
ret, rvec, tvec = solvePnP(obj_points, img_points, camera_matrix,
dist_coeffs)
dst, jac = Rodrigues(rvec)
# Make origin to camera matrix
"""
The origin_to_camera matrix looks like this:
dst[0,0] dst[0,1] dst[0,2] tvec[0,0]
dst[1,0] dst[1,1] dst[1,2] tvec[1,0]
dst[2,0] dst[2,1] dst[2,2] tvec[2,0]
0 0 0 1
"""
temp = np.append(dst, tvec, axis=1)
temp = np.append(temp, np.array([[0, 0, 0, 1]]), axis=0)
origin_to_camera = np.asmatrix(temp)
camera_to_origin = np.linalg.inv(origin_to_camera)
print("CAMERA TO ORIGIN: {}".format(camera_to_origin))
# Generate the location of the camera
# Seems to use a homogenous coordinates system (x,y,z,k)
gen_out = np.array([0, 0, 0, 1]).T
camera_coordinates = np.matmul(camera_to_origin, gen_out)
print("CAMERA COORDINATES: {}".format(camera_coordinates))
# write matrix to file
calib_file = open(calib_file_name, "a")
rows, cols = np.shape(camera_to_origin)
calib_file.write("transform_matrix =")
write_matrix_to_file(camera_to_origin, calib_file)
# Compute offsets via new calibration process
print("Axis calibration was successful!")
print("We will now center the camera. Place any apriltag where you would \
like (0,0) to be.")
print("A blue dot will appear in the center of the tag you placed to help \
show where (0,0) will be set to.")
print("When you have your tag in the right place, press SPACE.")
while True:
# Locate tag for use as origin
frame = get_image(camera)
gray = cvtColor(frame, COLOR_BGR2GRAY)
detections, det_image = detector.detect(gray, return_image=True)
if len(detections) == 0:
continue
(x_offset, y_offset, _,
_) = compute_tag_undistorted_pose(camera_matrix, dist_coeffs,
camera_to_origin, detections[0],
ORIGIN_TAG_SIZE)
cv2.circle(frame, (int(x_offset), int(y_offset)), 5, (255, 0, 0), 3)
imshow("Origin tag", frame)
if cv2.waitKey(1) & 0xFF == ord(" "):
break
else:
continue
# Write offsets
calib_file.write("offsets = ")
calib_file.write(str(-1 * x_offset))
calib_file.write(" ")
calib_file.write(str(-1 * y_offset))
calib_file.write("\n")
calib_file.close()
print("Finished writing transformation matrix to calibration file")
pass
def main():
# DEBUGGING AND TIMING VARIABLES
past_time = -1 # time to start counting. Set just before first picture taken
num_frames = 0 # number of frames processed
args = get_args()
url = args['url']
SEND_DATA = (args['url'] != None)
calib_file_name = args['file']
TAG_SIZE = args['size']
calib_file = open(calib_file_name)
camera = VideoCapture(DEVICE_ID) # Open the camera & set camera arams
if (not VideoCapture.isOpened(camera)):
print("Failed to open video capture device")
exit(0)
camera.set(CAP_PROP_FRAME_WIDTH, 1280)
camera.set(CAP_PROP_FRAME_HEIGHT, 720)
camera.set(CAP_PROP_FPS, 30)
# Get matrices from calibration file
print("Parsing calibration file " + calib_file_name + "...")
calib_file, camera_matrix, dist_coeffs = get_matrices_from_file(
calib_file_name)
transform_matrix = get_transform_matrix(calib_file)
x_offset, y_offset = get_offsets_from_file(calib_file)
calib_file.close()
assert (camera_matrix.shape == (3, 3))
assert (dist_coeffs.shape == (1, 5))
assert (transform_matrix.shape == (4, 4))
# print("TRANSFORM MATRIX:\n {}\n".format(transform_matrix))
print("Calibration file parsed successfully.")
print("Initializing apriltag detector...")
# make the detector
detector = apriltag.Detector(searchpath=apriltag._get_demo_searchpath())
frame = []
gray = []
print("Detector initialized")
print("")
print("The program will begin sending data in 3 seconds.")
print("Press CTRL+C to stop this program.")
time.sleep(3)
print("Starting detection")
img_points = np.ndarray((4, 2)) # 4 2D points
obj_points = np.ndarray((4, 3)) # 4 3D points
detections = []
past_time = time.time()
while True:
if not camera.isOpened():
print("Failed to open camera")
exit(0)
# take a picture and get detections
frame = get_image(camera)
gray = cvtColor(frame, COLOR_BGR2GRAY)
# Un-distorting an image worsened distortion effects
# Uncomment this if needed
# dst = undistort_image(frame, camera_matrix, dist_coeffs)
# gray = cvtColor(dst, COLOR_BGR2GRAY)
detections, det_image = detector.detect(gray, return_image=True)
if len(detections) == 0:
continue # Try again if we don't get anything
print("Found " + str(len(detections)) + " apriltags")
for d in detections:
# TODO draw tag - might be better to generalize, because
# locate_cameras does this too.
(x, y, z, angle) = compute_tag_undistorted_pose(
camera_matrix, dist_coeffs, transform_matrix, d, TAG_SIZE)
# Scale the coordinates, and print for debugging
# prints Device ID :: tag id :: x y z angle
# TODO debug offset method - is better, but not perfect.
x = MULT_FACTOR * (x + x_offset)
y = MULT_FACTOR * (y + y_offset)
# print(tag_xyz)
print("{} :: {} :: {} {} {} {}".format(
DEVICE_ID, d.tag_id, x, y, z, angle))
# Send the data to the URL specified.
# This is usually a URL to the base station.
if SEND_DATA:
payload = {
"id": d.tag_id,
"x": x,
"y": y,
"orientation": angle
}
r = requests.post(url, json=payload)
status_code = r.status_code
if status_code / 100 != 2:
# Status codes not starting in '2' are usually error codes.
print("WARNING: Basestation returned status code {}".format(
status_code))
else:
num_frames += 1
print("Vision FPS (Vision System outflow): {}".format(
num_frames / (time.time() - past_time)))
pass
def main():
global target_x
global target_y
global target_angle
global turning_radius
DIM = (800, 600)
# USB Camera Parameters
K=np.array([[646.986342788419, 0.0, 383.9135552285603], [0.0, 647.4588452248628, 315.82293891405163], [0.0, 0.0, 1.0]])
D=np.array([[0.40302428743352114], [0.10116712172043976], [0.6206370706341585], [-4.18627051202362]])
camera_params = (646.986342788419,647.4588452248628,383.9135552285603,315.82293891405163)
video_capture = cv2.VideoCapture(0)
video_capture.set(cv2.CAP_PROP_FRAME_HEIGHT,600)
video_capture.set(cv2.CAP_PROP_FRAME_WIDTH,800)
map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), K, DIM, cv2.CV_16SC2)
detector = apriltag.Detector(searchpath=apriltag._get_demo_searchpath())
tag_size = 160
target_x = np.int(input("key in target x coordinates "))
target_y = np.int(input("key in target y coordinates "))
target_angle = np.float(input("key in target angle (in degrees from -180 to 180) "))
turning_radius = np.int(input("key in turning radius (in pixels) "))
print("target point is "+"("+str(target_x)+","+str(target_y)+")")
print("traget angle is "+str(target_angle))
print("turning radius is "+str(turning_radius))
cv2.namedWindow('image')
cv2.setMouseCallback('image', set_target)
#target_right = (np.int(target_x +math.cos(math.radians(target_angle))*turning_radius),np.int(target_y +math.sin(math.radians(target_angle))*turning_radius))
#target_left = (np.int(target_x -math.cos(math.radians(target_angle))*turning_radius),np.int(target_y -math.sin(math.radians(target_angle))*turning_radius))
while (True):
_, frame = video_capture.read()
gray_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
gray_frame = cv2.remap(gray_frame, map1, map2, interpolation = cv2.INTER_LINEAR,borderMode=cv2.BORDER_CONSTANT)
result = detector.detect(gray_frame)
target_right = (np.int(target_x +math.cos(math.radians(target_angle))*turning_radius),np.int(target_y +math.sin(math.radians(target_angle))*turning_radius))
target_left = (np.int(target_x -math.cos(math.radians(target_angle))*turning_radius),np.int(target_y -math.sin(math.radians(target_angle))*turning_radius))
cv2.circle(gray_frame,target_right,turning_radius,(255,255,255),-1)
cv2.circle(gray_frame,target_left,turning_radius,(0,0,0),-1)
if (result):
start_point = result[0].center.astype(int)
start_angle = math.degrees(math.atan2(result[0].corners[1][1]-result[0].corners[0][1],result[0].corners[1][0]-result[0].corners[0][0]))
curr_right = (np.int(start_point[0]+math.cos(math.radians(start_angle))*turning_radius),np.int(start_point[1]+math.sin(math.radians(start_angle))*turning_radius))
curr_left = (np.int(start_point[0]-math.cos(math.radians(start_angle))*turning_radius),np.int(start_point[1]-math.sin(math.radians(start_angle))*turning_radius))
cv2.circle(gray_frame,curr_right,turning_radius,(255,255,255),-1)
cv2.circle(gray_frame,curr_left,turning_radius,(0,0,0),-1)
dist = np.array([ np.linalg.norm(np.array(target_right)-np.array(curr_right)),
np.linalg.norm(np.array(target_right)-np.array(curr_left)),
np.linalg.norm(np.array(target_left)-np.array(curr_right)),
np.linalg.norm(np.array(target_left)-np.array(curr_left))])
dist[dist<=2*turning_radius] = 100000
index = np.argmin(dist)
if index == 0:
dest_angle = math.degrees(math.atan2(target_right[0]-curr_right[0],target_right[1]-curr_right[1]))
dest_angle = (180-dest_angle) if dest_angle>0 else -180-dest_angle
pt1 = (np.int(curr_right[0]-math.cos(math.radians(dest_angle))*turning_radius),np.int(curr_right[1]-math.sin(math.radians(dest_angle))*turning_radius))
pt2 = (np.int(target_right[0]-math.cos(math.radians(dest_angle))*turning_radius),np.int(target_right[1]-math.sin(math.radians(dest_angle))*turning_radius))
cv2.line(gray_frame,pt1,pt2,(0,0,0),5)
cv2.line(gray_frame,target_right,curr_right,(0,0,0),5)
elif index == 1:
dest_angle = math.degrees(math.atan2(target_right[0]-curr_left[0],target_right[1]-curr_left[1]))
dest_angle = (180-dest_angle) if dest_angle>0 else -180-dest_angle
dest_angle = dest_angle + math.degrees(math.acos(turning_radius/(dist[1]/2)))+90
pt1 = (np.int(curr_left[0]-math.cos(math.radians(dest_angle))*turning_radius),np.int(curr_left[1]-math.sin(math.radians(dest_angle))*turning_radius))
pt2 = (np.int(target_right[0]+math.cos(math.radians(dest_angle))*turning_radius),np.int(target_right[1]+math.sin(math.radians(dest_angle))*turning_radius))
cv2.line(gray_frame,pt1,pt2,(255,255,255),5)
cv2.line(gray_frame,target_right,curr_left,(0,0,0),5)
elif index == 2:
dest_angle = math.degrees(math.atan2(target_left[0]-curr_right[0],target_left[1]-curr_right[1]))
dest_angle = (180-dest_angle) if dest_angle>0 else -180-dest_angle
dest_angle = dest_angle - math.degrees(math.acos(turning_radius/(dist[2]/2)))+ 90
pt1 = (np.int(curr_right[0]-math.cos(math.radians(dest_angle))*turning_radius),np.int(curr_right[1]-math.sin(math.radians(dest_angle))*turning_radius))
pt2 = (np.int(target_left[0]+math.cos(math.radians(dest_angle))*turning_radius),np.int(target_left[1]+math.sin(math.radians(dest_angle))*turning_radius))
cv2.line(gray_frame,pt1,pt2,(255,255,255),5)
cv2.line(gray_frame,target_left,curr_right,(0,0,0),5)
else:
dest_angle = math.degrees(math.atan2(target_left[0]-curr_left[0],target_left[1]-curr_left[1]))
dest_angle = (180-dest_angle) if dest_angle>0 else -180-dest_angle
pt1 = (np.int(curr_left[0]+math.cos(math.radians(dest_angle))*turning_radius),np.int(curr_left[1]+math.sin(math.radians(dest_angle))*turning_radius))
pt2 = (np.int(target_left[0]+math.cos(math.radians(dest_angle))*turning_radius),np.int(target_left[1]+math.sin(math.radians(dest_angle))*turning_radius))
cv2.line(gray_frame,pt1,pt2,(0,0,0),5)
cv2.line(gray_frame,target_left,curr_left,(0,0,0),5)
cv2.imshow("image",gray_frame)
key = cv2.waitKey(1)&0xFF
if key == ord('q'):
break
elif key == ord('w'):
if turning_radius < 100:
turning_radius+=1
elif key == ord('s'):
if turning_radius > 10:
turning_radius-=1
elif key == ord('a'):
target_angle -= 5
if target_angle <= -180:
target_angle = 180
elif key == ord('d'):
target_angle += 5
if target_angle >= 180:
target_angle = -180
video_capture.release()
cv2.destroyAllWindows()
def Main():
reference_tags = [0, 1, 2]
x_distance = 95 # 2.1844
y_distance = 67.5 # 1.1938
global odoData
global ref_x
global ref_y
global orig
global rotation_matrix
global transform_matrix
print(odoData)
odoData1 = {'robot1': 170, 'robot2': 650}
window = 'Overlay1'
cv2.namedWindow(window)
endpt = [0, 0]
endptFlag = True
HOST = '192.168.1.78' # The server's hostname or IP address
PORT = 12346
SERVER_SOCKET = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
SERVER_SOCKET.bind((HOST, PORT))
print('Socket Bound to Port ', PORT)
SERVER_SOCKET.listen(5)
# Starts a thread that looks for connections form the swarmbots
connection_thread = threading.Thread(target=find_connections,
args=(SERVER_SOCKET, ))
connection_thread.start()
# Localization Code Here
parser = ArgumentParser(description='test apriltag Python bindings')
parser.add_argument('device_or_movie', metavar='INPUT', nargs='?',
default=0,
help='Movie to load or integer ID of camera device'
)
orient = 0
apriltag.add_arguments(parser)
options = parser.parse_args()
try:
try:
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1920)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080)
except ValueError:
cap = cv2.VideoCapture(options.device_or_movie)
detector = apriltag.Detector(options,
searchpath=apriltag._get_demo_searchpath())
font = cv2.FONT_HERSHEY_SIMPLEX
fontScale = 0.3
fontColor = (0, 0, 255)
lineType = 1
while True:
odoData1.clear()
(success, frame) = cap.read()
if not success:
break
gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
(detections, dimg) = detector.detect(gray,
return_image=True)
headDir = np.array([0, 0])
num_detections = len(detections)
dimg1 = dimg
try:
# Gets the x and y positions of the reference tags
ref_x = detections[reference_tags[1]].center
ref_y = detections[reference_tags[2]].center
orig = detections[reference_tags[0]].center
except IndexError:
continue
# Creates the transform matrix for tags (units are inches)
transform_matrix.append(np.abs(x_distance)
/ np.abs(ref_x[0] - orig[0]))
transform_matrix.append(np.abs(y_distance) / np.abs(orig[1]
- ref_y[1]))
# Creates the rotaion matrix for the reference frame
rotation_matrix = np.array([[0, 1], [-1, 0]])
for (i, detection) in enumerate(detections):
dimg1 = draw(frame, detection.corners)
center = detection.center
# Gets the center of the tag in inches and rotated accordingly
center_meters = transform(center)
# Labels are the position in inches
posString = '({x:.2f},{y:.2f})'.format(x=center_meters[0],y=center_meters[1])
if not detection.tag_id in reference_tags:
# Gets the forward direction
(forwardDir, angle) = headingDir(detection.corners,
center)
# Converts the forward direction to meters
forwardDir_meters = transform(forwardDir)
# Only draws arrows on tags that are not references
dimg1 = draw1(dimg1, forwardDir, center, (0, 0,
255))
centerTxt = center.ravel().astype(int).astype(str)
cv2.putText(dimg1,posString,tuple(center.ravel().astype(int) + 10),font,fontScale,(255, 0, 0),lineType,)
if detection.tag_id == int(endPtID):
dimg1 = cv2.putText(dimg1,'End Point',tuple(center.ravel().astype(int)),font,0.8,fontColor,2,)
dimg1 = cv2.circle(dimg1,tuple(center.ravel().astype(int)), 30, (0,128, 255), 2)
else:
cv2.putText(dimg1,'Id:' + str(detection.tag_id),tuple(center.ravel().astype(int)),font,0.8,(0, 0, 0),2,)
overlay = dimg1
if not detection.tag_id in reference_tags:
# Gets the center position, heading vector and current angle of all none reference tags
odoData1[str(detection.tag_id)] = \
[tuple(center_meters)
+ tuple(forwardDir_meters), angle]
else:
# Only gets the center of reference tags
odoData1[str(detection.tag_id)] = \
[tuple(center_meters)]
if len(detections) == 0 and odoData1 == 0:
overlay = frame
odoData = odoData1.copy()
elif odoData1:
odoData = odoData1.copy()
# print('frame')
cv2.imshow(window, overlay)
cv2.waitKey(1)
except KeyboardInterrupt:
# Change the following line to get back the connection part.
# sndString=str(strtPt[0])+' '+str(strtPt[1])+' '+str(headDir[0])+' '+str(headDir[1])+' '+str(endptFlag)+' '+str(endpt[0])+' '+str(endpt[1])
# k = cv2.waitKey(1)
return
def main():
global target_x
global target_y
global target_angle
global turning_radius
# ROS Publisher Init
pub = rospy.Publisher('watch_tower_control',
watch_tower_control,
queue_size=1)
rospy.init_node('custom_talker', anonymous=True)
control_signal = watch_tower_control()
DIM = (800, 600)
# USB Camera Parameters
K = np.array([[646.986342788419, 0.0, 383.9135552285603],
[0.0, 647.4588452248628, 315.82293891405163],
[0.0, 0.0, 1.0]])
D = np.array([[0.40302428743352114], [0.10116712172043976],
[0.6206370706341585], [-4.18627051202362]])
camera_params = (646.986342788419, 647.4588452248628, 383.9135552285603,
315.82293891405163)
video_capture = cv2.VideoCapture(0)
video_capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 600)
video_capture.set(cv2.CAP_PROP_FRAME_WIDTH, 800)
map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), K, DIM,
cv2.CV_16SC2)
detector = apriltag.Detector(searchpath=apriltag._get_demo_searchpath())
tag_size = 160
target_x = 300
target_y = 200
target_angle = 0
turning_radius = 30
print("target point is " + "(" + str(target_x) + "," + str(target_y) + ")")
print("traget angle is " + str(target_angle))
print("turning radius is " + str(turning_radius))
cv2.namedWindow('image')
cv2.setMouseCallback('image', set_target)
while (True):
_, frame = video_capture.read()
gray_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
gray_frame = cv2.remap(gray_frame,
map1,
map2,
interpolation=cv2.INTER_LINEAR,
borderMode=cv2.BORDER_CONSTANT)
result = detector.detect(gray_frame)
target_right = (
np.int(target_x +
math.cos(math.radians(target_angle)) * turning_radius),
np.int(target_y +
math.sin(math.radians(target_angle)) * turning_radius))
target_left = (
np.int(target_x -
math.cos(math.radians(target_angle)) * turning_radius),
np.int(target_y -
math.sin(math.radians(target_angle)) * turning_radius))
cv2.circle(gray_frame, target_right, turning_radius, (255, 255, 255),
-1)
cv2.circle(gray_frame, target_left, turning_radius, (0, 0, 0), -1)
if (result):
start_point = result[0].center.astype(int)
start_angle = math.degrees(
math.atan2(result[0].corners[1][1] - result[0].corners[0][1],
result[0].corners[1][0] - result[0].corners[0][0]))
curr_right = (
np.int(start_point[0] +
math.cos(math.radians(start_angle)) * turning_radius),
np.int(start_point[1] +
math.sin(math.radians(start_angle)) * turning_radius))
curr_left = (
np.int(start_point[0] -
math.cos(math.radians(start_angle)) * turning_radius),
np.int(start_point[1] -
math.sin(math.radians(start_angle)) * turning_radius))
cv2.circle(gray_frame, curr_right, turning_radius, (255, 255, 255),
-1)
cv2.circle(gray_frame, curr_left, turning_radius, (0, 0, 0), -1)
dist = np.array([
np.linalg.norm(np.array(target_right) - np.array(curr_right)),
np.linalg.norm(np.array(target_right) - np.array(curr_left)),
np.linalg.norm(np.array(target_left) - np.array(curr_right)),
np.linalg.norm(np.array(target_left) - np.array(curr_left))
])
dist[dist <= 2 * turning_radius] = 100000
index = np.argmin(dist)
if np.linalg.norm(
np.array([target_x, target_y]) -
np.array(start_point)) < 10:
control_signal.control = [0, speed_center]
else:
control_signal.control = [0, max_speed]
time.sleep(0.01)
if index == 0:
dest_angle = math.degrees(
math.atan2(target_right[0] - curr_right[0],
target_right[1] - curr_right[1]))
dest_angle = (
180 - dest_angle) if dest_angle > 0 else -180 - dest_angle
pt1 = (np.int(curr_right[0] -
math.cos(math.radians(dest_angle)) *
turning_radius),
np.int(curr_right[1] -
math.sin(math.radians(dest_angle)) *
turning_radius))
pt2 = (np.int(target_right[0] -
math.cos(math.radians(dest_angle)) *
turning_radius),
np.int(target_right[1] -
math.sin(math.radians(dest_angle)) *
turning_radius))
temp_angle = -math.degrees(
math.atan2(pt1[0] - pt2[0], pt1[1] - pt2[1]))
cv2.line(gray_frame, pt1, pt2, (0, 0, 0), 5)
cv2.line(gray_frame, target_right, curr_right, (0, 0, 0), 5)
if (abs(start_angle - temp_angle) < 10):
control_signal.control[0] = center
#pwm.set_pwm(1, 0, center)
else:
control_signal.control[0] = left_max
#pwm.set_pwm(1, 0, left_max)
time.sleep(0.01)
elif index == 1:
dest_angle = math.degrees(
math.atan2(target_right[0] - curr_left[0],
target_right[1] - curr_left[1]))
dest_angle = (
180 - dest_angle) if dest_angle > 0 else -180 - dest_angle
dest_angle = dest_angle + math.degrees(
math.acos(turning_radius / (dist[1] / 2))) + 90
pt1 = (np.int(curr_left[0] -
math.cos(math.radians(dest_angle)) *
turning_radius),
np.int(curr_left[1] -
math.sin(math.radians(dest_angle)) *
turning_radius))
pt2 = (np.int(target_right[0] +
math.cos(math.radians(dest_angle)) *
turning_radius),
np.int(target_right[1] +
math.sin(math.radians(dest_angle)) *
turning_radius))
temp_angle = -math.degrees(
math.atan2(pt1[0] - pt2[0], pt1[1] - pt2[1]))
cv2.line(gray_frame, pt1, pt2, (255, 255, 255), 5)
cv2.line(gray_frame, target_right, curr_left, (0, 0, 0), 5)
if (abs(start_angle - temp_angle) < 10):
control_signal.control[0] = center
#pwm.set_pwm(1, 0, center)
else:
control_signal.control[0] = right_max
#pwm.set_pwm(1, 0, right_max)
time.sleep(0.01)
#print(start_angle,temp_angle)
elif index == 2:
dest_angle = math.degrees(
math.atan2(target_left[0] - curr_right[0],
target_left[1] - curr_right[1]))
dest_angle = (
180 - dest_angle) if dest_angle > 0 else -180 - dest_angle
dest_angle = dest_angle - math.degrees(
math.acos(turning_radius / (dist[2] / 2))) + 90
pt1 = (np.int(curr_right[0] -
math.cos(math.radians(dest_angle)) *
turning_radius),
np.int(curr_right[1] -
math.sin(math.radians(dest_angle)) *
turning_radius))
pt2 = (np.int(target_left[0] +
math.cos(math.radians(dest_angle)) *
turning_radius),
np.int(target_left[1] +
math.sin(math.radians(dest_angle)) *
turning_radius))
temp_angle = -math.degrees(
math.atan2(pt1[0] - pt2[0], pt1[1] - pt2[1]))
cv2.line(gray_frame, pt1, pt2, (255, 255, 255), 5)
cv2.line(gray_frame, target_left, curr_right, (0, 0, 0), 5)
if (abs(start_angle - temp_angle) < 10):
control_signal.control[0] = center
#pwm.set_pwm(1, 0, center)
else:
control_signal.control[0] = left_max
#pwm.set_pwm(1, 0, left_max)
time.sleep(0.01)
#print(start_angle,temp_angle)
else:
dest_angle = math.degrees(
math.atan2(target_left[0] - curr_left[0],
target_left[1] - curr_left[1]))
dest_angle = (
180 - dest_angle) if dest_angle > 0 else -180 - dest_angle
pt1 = (np.int(curr_left[0] +
math.cos(math.radians(dest_angle)) *
turning_radius),
np.int(curr_left[1] +
math.sin(math.radians(dest_angle)) *
turning_radius))
pt2 = (np.int(target_left[0] +
math.cos(math.radians(dest_angle)) *
turning_radius),
np.int(target_left[1] +
math.sin(math.radians(dest_angle)) *
turning_radius))
temp_angle = -math.degrees(
math.atan2(pt1[0] - pt2[0], pt1[1] - pt2[1]))
cv2.line(gray_frame, pt1, pt2, (0, 0, 0), 5)
cv2.line(gray_frame, target_left, curr_left, (0, 0, 0), 5)
if (abs(start_angle - temp_angle) < 10):
control_signal.control[0] = center
#pwm.set_pwm(1, 0, center)
else:
control_signal.control[0] = right_max
#pwm.set_pwm(1, 0, right_max)
time.sleep(0.01)
#print(start_angle,temp_angle)
else:
control_signal.control = [center, 400]
#pwm.set_pwm(2, 0, 400)
pub.publish(control_signal)
cv2.imshow("image", gray_frame)
key = cv2.waitKey(1) & 0xFF
if key == ord('q'):
break
elif key == ord('w'):
if turning_radius < 100:
turning_radius += 1
elif key == ord('s'):
if turning_radius > 10:
turning_radius -= 1
elif key == ord('a'):
target_angle -= 5
if target_angle <= -180:
target_angle = 180
elif key == ord('d'):
target_angle += 5
if target_angle >= 180:
target_angle = -180
video_capture.release()
cv2.destroyAllWindows()
control_signal.control = [center, 400]
pub.publish(control_signal)
