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():
'''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():
cam = cv2.VideoCapture(0)
cam.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
cam.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)
# sleep(0.2)
success, frame = cam.read()
if not success:
print 'failed to read camera'
return
display = frame.copy()
frame = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
cv2.imshow('win', frame)
cv2.waitKey()
aprilsize = 0.08
opts = np.array([[aprilsize, -aprilsize, 0.],
[-aprilsize, -aprilsize, 0.],
[-aprilsize, aprilsize, 0.],
[aprilsize, aprilsize, 0.]])
cmat = np.loadtxt('CameraMatrix.txt')
dmat = np.loadtxt('Distortion.txt')
# set upt the detector
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()
# create an apriltag detector
detector = apriltag.Detector(options)
tags = detector.detect(frame, return_image= False)
print 'found {} tags'.format(len(tags))
print
for tag in tags:
print 'tag has corners \n{} \n'.format(tag.corners)
print tag.corners.shape, tag.corners.dtype
# line = np.array([[10, 0], [10, 470]])
# cv2.polylines(display, [line], True, (255, 0, 255), 0, 16)
cv2.polylines(display, [tag.corners.astype(int)], True, (255,255,0), 1, 16)
# print frame.shape
# corners_adj = tag.corners + np.array([frame.shape[0]/2, 0])
# print 'adjusted corners \n{} \n'.format(corners_adj)
retval, rvec, tvec = cv2.solvePnP(opts, tag.corners,
cmat, dmat)
print 'tag has tvec \n{}'.format(tvec)
def apriltag_image(
input_images=[
'../media/input/single_tag.jpg', '../media/input/multiple_tags.jpg'
],
output_images=False,
display_images=True,
detection_window_name='AprilTag',
):
'''
Detect AprilTags from static images.
Args: input_images [list(str)]: List of images to run detection algorithm on
output_images [bool]: Boolean flag to save/not images annotated with detections
display_images [bool]: Boolean flag to display/not images annotated with detections
detection_window_name [str]: Title of displayed (output) tag detection window
'''
parser = ArgumentParser(description='Detect AprilTags from static images.')
apriltag.add_arguments(parser)
options = parser.parse_args()
'''
Set up a reasonable search path for the apriltag DLL.
Either install the DLL in the appropriate system-wide
location, or specify your own search paths as needed.
'''
detector = apriltag.Detector(options, searchpath=apriltag._get_dll_path())
for image in input_images:
img = cv2.imread(image)
print('Reading {}...\n'.format(os.path.split(image)[1]))
result, overlay = apriltag.detect_tags(
img,
detector,
camera_params=(3156.71852, 3129.52243, 359.097908, 239.736909),
tag_size=0.0762,
vizualization=3,
verbose=3,
annotation=True)
if output_images:
output_path = '../media/output/' + str(os.path.split(image)[1])
output_path = output_path.replace(str(os.path.splitext(image)[1]),
'.jpg')
cv2.imwrite(output_path, overlay)
if display_images:
cv2.imshow(detection_window_name, overlay)
while cv2.waitKey(5) < 0: # Press any key to load subsequent image
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)
detector = apriltag.Detector(options)
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 __init__(self, tag_size):
# The python bindings for the April Tag library require this strange configuration:
parser = ArgumentParser(description='test apriltag Python bindings')
parser.families = 'tag36h11'
parser.border = 1 # how wide (in bit-sizes) is the black border? (usually 1)
parser.nthreads = 4 #(int) number of threads to use
parser.quad_decimate = 1.0 #(float) Decimate input image by this factor
parser.quad_blur = 0.0 #(float) Apply low-pass blur to input; negative sharpens
parser.refine_edges = True #(bool) Set to True to spend more time to align edges of tags
parser.refine_decode = True #(bool) Set to True to spend more time to decode tags
parser.refine_pose = True #(bool) Set to True to spend more time to precisely localize tags
parser.debug = False #(bool)Enable/Disable debugging output (slow when enabled)
parser.quad_contours = True
apriltag.add_arguments(parser)
self.options = parser.parse_args()
self.options.tag_size = tag_size
def __init__(self):
rospy.init_node('apriltag_detector')
# rospy.Subscriber('image_topic', Image, self.image_callback)
# HACK: rather than subscribing to an image topic and then using that
# data, just capture the node with cv2.VideoCapture every set length
# of time.
# Will let us convert from cv image to ros image and back
self.bridge = CvBridge()
# UNHACK: subscribe to the image publisher and create an image_callback
self.sub = rospy.Subscriber("camera_raw", Image, self.image_callback )
# set up parameters for the apriltag detector
aprilsize = 0.08 # half the side length of the apriltag in meters; changed from 0.05 4/20/17
self.opts = np.array([[aprilsize, -aprilsize, 0.],
[-aprilsize, -aprilsize, 0.],
[-aprilsize, aprilsize, 0.],
[aprilsize, aprilsize, 0.]])
# From camera calibration
self.cmatrix = np.loadtxt("/home/schleppy/project_files/Schlepbot-E90/catkin_ws_new/src/apriltag_detector/CameraMatrix.txt")
self.dists = np.loadtxt("/home/schleppy/project_files/Schlepbot-E90/catkin_ws_new/src/apriltag_detector/Distortion.txt")
self.pub = rospy.Publisher('apriltag', Point, queue_size = 10)
# set up the detector
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(rospy.myargv()[1:])
# create an apriltag detector
self.detector = apriltag.Detector(options)
# Moving average of translation vectors.
self.tvec_avg = np.array([[0], [0], [0]])
def main():
'''Main function.'''
parser = ArgumentParser(
description='test apriltag Python bindings')
apriltag.add_arguments(parser)
options = parser.parse_args()
for k in range(20):
def __init__(self):
rospy.init_node('apriltag_detector')
rospy.Subscriber('image_topic_raw', Image)
self.pub = rospy.Publisher('apriltag', Image, self.image_callback)
self.bridge = CvBridge()
# set upt the detector
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()
# create an apriltag detector
self.detector = apriltag.Detector(options)
def main():
'''Detect apriltags.'''
if (use_pympler):
tr = tracker.SummaryTracker()
from argparse import ArgumentParser
# for some reason pylint complains about members being undefined :(
# pylint: disable=E1101
parser = ArgumentParser(
description='Detect apriltag in videos and images. Output to tagjson/ directory')
show_default = ' (default %(default)s)'
apriltag.add_arguments(parser)
parser.add_argument('filenames', metavar='IMAGE', nargs='*',
help='files to convert')
parser.add_argument('-F', '--family-preset',
action=FamilyPresetAction,
help='Family preset: detect "inv" at the end of the name to set both "family" and "inverse"')
parser.add_argument('-V', dest='video_in', default=None,
help='Input video')
parser.add_argument('-f0', dest='f0', default=0, type=int,
help='Frame start '+ show_default)
parser.add_argument('-f1', dest='f1', default=0, type=int,
help='Frame end '+ show_default)
parser.add_argument('-fps', dest='fps', default=20.0, type=float,
help='fps '+ show_default)
parser.add_argument('-outdir', dest='outdir', default='.',
help='For video, basedir of output '+ show_default)
parser.add_argument('-tagout', dest='tagout', default=False,
action='store_true',
help='Save image with detected tags overlay '+ show_default)
parser.add_argument('-tagplot', dest='tagplot', default=False,
action='store_true',
help='Plot tags overlay using matplotlib '+ show_default)
parser.add_argument('-m', dest='multiframefile', default=False,
action='store_true',
help='Save multiple frames into single JSON file '+ show_default)
parser.add_argument('-mv', dest='multiframefile_version', default=0, type=int,
help='Single JSON file version'+ show_default)
parser.add_argument('-cvt', dest='cv_nthreads', default=2, type=int,
help='Number of threads for OpenCV '+ show_default)
if (True):
parser.add_argument('-max_side_length', dest='max_side_length',
default=35, type=int,
help='Maximum tag size in pixel '+ show_default)
parser.add_argument('-min_side_length', dest='min_side_length',
default=20, type=int,
help='Tags smaller than that are discarded '+ show_default)
parser.add_argument('-min_aspect', dest='min_aspect',
default=0.7, type=float,
help='Tags smaller than that are discarded '+ show_default)
parser.add_argument('-rgb_mean', dest='rgb_mean', default=False,
action='store_true',
help='Extract extra info rgb_mean '+ show_default)
parser.add_argument('-tag_img', dest='tag_img', default=False,
action='store_true',
help='Extract extra info tag image '+ show_default)
parser.add_argument('-tag_os', dest='tag_oversampling',
default=1, type=int,
help='Oversampling factor to extract tag image or compute rgb_mean '+ show_default)
parser.add_argument('-tag_d', dest='tag_d',
default=5, type=int,
help='Size of tag (tag25h5 -> d=5) '+ show_default)
options = parser.parse_args()
cv2.setNumThreads(options.cv_nthreads)
print('OpenCV running with {} threads'.format(cv2.getNumThreads()))
if (not options.multiframefile and options.multiframefile_version>0):
options.multiframefile = True
if (options.multiframefile and options.multiframefile_version==0):
options.multiframefile_version = 2
if (options.f1<options.f0):
options.f1=options.f0
print(options)
det = apriltag.Detector(options)
# For Quad Contour Params Detection (QCP)
#det.tag_detector.contents.qcp.min_side_length = options.min_side_length;
#det.tag_detector.contents.qcp.min_aspect = options.min_aspect;
# Defaults for Quad Threshold Approach
# qtp->max_nmaxima = 10;
# qtp->min_cluster_pixels = 5;
# qtp->max_line_fit_mse = 1.0;
# qtp->critical_rad = 10 * M_PI / 180;
# qtp->deglitch = 0;
# qtp->min_white_black_diff = 15;
qtp = det.tag_detector.contents.qtp
qtp.min_cluster_pixels = 200
qtp.critical_rad = 50.0 / 180 * math.pi
qtp.min_white_black_diff = 30
qtp.deglitch = 0
# ('max_nmaxima', ctypes.c_int),
# ('max_line_fit_mse', ctypes.c_float),
#init_gui()
if not (options.video_in is None): # Input is a video
print('Processing video {}'.format(options.video_in))
vidcap = cv2.VideoCapture(options.video_in)
if not vidcap.isOpened():
print("Could not open video. Aborting.")
raise IOError("Could not open video {}. Aborting.".format(options.video_in))
#sys.exit(1)
vidcap.set(cv2.CAP_PROP_POS_FRAMES,0)
nframes=vidcap.get(cv2.CAP_PROP_FRAME_COUNT);
coderfps=vidcap.get(cv2.CAP_PROP_FPS)
vidcap.set(cv2.CAP_PROP_POS_FRAMES,nframes-1)
duration=vidcap.get(cv2.CAP_PROP_POS_MSEC)/1000.0;
if (False):
print("Opened video.\n nframes={}\n nframes with cmd line fps={}\n coder fps={}\n coder video duration={}s={}min\n command line fps={}\n command line duration={}s={}min\n time at frame {}={}s".format(nframes, nframes*coderfps/options.fps,
coderfps,nframes/coderfps, nframes/coderfps/60,
options.fps,nframes/options.fps, nframes/options.fps/60,
int(nframes), duration))
print('ffprobe infos:')
import shlex, subprocess
cmdline = "ffprobe '{}' -show_streams -loglevel -8 | grep nb_frames=".format(options.video_in)
subprocess.call(cmdline,shell=True)
cmdline = "ffprobe '{}' -show_streams -loglevel -8 | grep duration=".format(options.video_in)
subprocess.call(cmdline,shell=True)
cmdline = "ffprobe '{}' -show_streams -loglevel -8 | grep frame_rate=".format(options.video_in)
subprocess.call(cmdline,shell=True)
outdir = options.outdir
tagjson = outdir+'/tagjson'
tagout = outdir+'/tagout'
os.makedirs(outdir,exist_ok=True)
if (not options.multiframefile):
os.makedirs(tagjson,exist_ok=True)
if (options.tagout):
os.makedirs(tagout,exist_ok=True)
def printfps(t, name):
print("Time {:10} = {:5.3f}s ({:4.1f} fps)".format(name, t, 1.0/t))
fps=options.fps
vidcap.set(cv2.CAP_PROP_POS_MSEC,0)
status,orig = vidcap.read();
# Caution: orig in BGR format by default
if (orig is None):
print('Warning: could not read frame {}'.format(f))
print('Aborting...')
raise IOError("Could not read frame {}. Aborting.".format(f))
#sys.exit(1)
print("Image size: {}".format(orig.shape))
if (options.multiframefile):
filenameJSON=outdir+"/tags_{:05d}-{:05d}.json".format(options.f0,options.f1)
singlejson=Multiframejson(filenameJSON)
singlejson.set_config(options)
singlejson.open()
print("All writes will be to tmp JSON file {}...".format(singlejson.tmpfile))
for f in range(options.f0,options.f1+1):
filename=tagout+"/tagout_{:05d}.png".format(f)
filenameJSON=tagjson+"/tags_{:05d}.json".format(f)
print("Processing frame {}".format(f), flush=True)
tstart = timer()
vidcap.set(cv2.CAP_PROP_POS_MSEC,1000.0/fps*f)
#status,_ = vidcap.read(orig); # Source of segfault?
status,orig = vidcap.read(); # Caution: BGR format !
# Caution: orig in BGR format by default
if (orig is None):
print('Warning: could not read frame {}'.format(f))
print('Aborting...')
break
endread = timer()
#printfps(endread - tstart, 'read')
#print('dodetect',flush=True,file=sys.stderr)
detections = do_detect(det, orig)
enddetect = timer()
#print('dodetect DONE',flush=True,file=sys.stderr)
#printfps(enddetect-endread, 'detect')
extractextra = options.rgb_mean or options.tag_img
if (extractextra):
tag_family_d=options.tag_d # d=code pixels, d+2=code+inborder, d+4=code+outborder
tag_oversampling=options.tag_oversampling
flags = {'tag_img': options.tag_img, 'rgb_mean': options.rgb_mean}
tagextra = do_extract_extras(detections, orig,
flags=flags,
n=tag_family_d+4, pixsize=tag_oversampling)
else:
tagextra = None
endextra = timer()
if (options.multiframefile):
if (options.debug & 1): # DEBUG_LOG
print(" Appending to JSON file {}...".format(singlejson.tmpfile))
singlejson.append(detections, f, tagextra)
else:
print(" Saving JSON to {}...".format(filenameJSON))
savejson(detections, filenameJSON)
#plot_detections(detections, gax[0], orig)
if (options.tagout):
print(" Saving tagimg to {}".format(filename))
tagimg = orig.copy()
draw_detections(tagimg, detections, draw_sampling=0, fontscale=0.75, options=options)
#tagimg = cv2.cvtColor(tagimg, cv2.COLOR_RGB2BGR);
cv2.imwrite(filename,tagimg)
#print_detections(detections, show_details=False)
#print('Detected {} tags'.format(len(detections)))
#plt.draw()
#plt.show(block=False)
#_ = input("Stopped at frame {}. Press Enter to continue".format(f))
endsave = timer()
#printfps(endsave-enddetect,'save')
#printfps(endsave-tstart, 'TOTAL')
if (False):
print(' frame {:5}'.format(f))
else:
print(' DONE frame {:5}, {:3} tags, {:4.1f} fps'.format(f, len(detections), 1.0/(endsave-tstart)))
#print(' Times: {:5.1f} read, {:5.1f} detect, {:5.1f} extra, {:5.1f} save, {:5.1f} total'.format(
# endread-tstart, enddetect-endread, endextra-enddetect, endsave-endextra,
# endsave-tstart))
if (use_resource):
maxrss = resource.getrusage(resource.RUSAGE_SELF).ru_maxrss
print(' MEM frame {:5}, {:10d} bytes'.format(f, maxrss))
if (use_pympler):
tr.print_diff()
if (options.multiframefile):
print(" Closing JSON {}".format(singlejson.tmpfile))
singlejson.close()
print(" Renamed to {}".format(singlejson.filename))
else: # Input is an image (or several)
print('Processing image(s) {}'.format(options.filenames))
for filename in options.filenames:
filenameJSON="{filename}-tag.json".format(filename=filename)
filename_tagout="{filename}-out.jpg".format(filename=filename)
if have_cv2:
orig = cv2.imread(filename)
else:
pil_image = Image.open(filename)
orig = numpy.array(pil_image)
#gray = numpy.array(pil_image.convert('L'))
if (orig is None):
print('Warning: could not read frame {}'.format(f))
print('Aborting...')
break
detections = do_detect(det, orig)
print(" Detected {} tags".format(len(detections)))
extractextra = options.rgb_mean or options.tag_img
if (extractextra):
tag_family_d=options.tag_d
tag_oversampling=options.tag_oversampling
flags = {'tag_img': options.tag_img, 'rgb_mean': options.rgb_mean}
tagextra = do_extract_extras(detections, orig,
flags=flags,
n=tag_family_d+4, pixsize=tag_oversampling)
# print(tagextra)
else:
tagextra = None
if (options.tagplot):
ax=plt.gca()
plot_detections(detections, ax, orig)
if (options.tagout):
print(" Saving tagimg to {}".format(filename_tagout))
tagimg = orig.copy()
draw_detections(tagimg, detections, draw_sampling=0)
#tagimg = cv2.cvtColor(tagimg, cv2.COLOR_RGB2BGR);
cv2.imwrite(filename_tagout,tagimg)
if (options.tag_img):
for k,t in enumerate(tagextra):
filename_tagimg = "{filename}-tagimg-{k}-{id}.png".format(filename=filename,k=k,id=detections[k].tag_id)
cv2.imwrite(filename_tagimg,t['tag_img'] )
print(" Saving JSON to {}".format(filenameJSON))
savejson(detections, filenameJSON)
pass
####################################
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)
def main():
'''Test function for this Python wrapper.'''
from argparse import ArgumentParser
# for some reason pylint complains about members being undefined :(
# pylint: disable=E1101
parser = ArgumentParser(description='test apriltag Python bindings')
show_default = ' (default %(default)s)'
parser.add_argument('filenames',
metavar='IMAGE',
nargs='*',
help='files to convert')
parser.add_argument('-V',
dest='video_in',
default=None,
help='Input video')
parser.add_argument('-f0',
dest='f0',
default=0,
type=int,
help='Frame start ' + show_default)
parser.add_argument('-f1',
dest='f1',
default=0,
type=int,
help='Frame end ' + show_default)
parser.add_argument('-min_side_length',
dest='min_side_length',
default=25,
type=int,
help='Tags smaller than that are discarded ' +
show_default)
parser.add_argument('-min_aspect',
dest='min_aspect',
default=0.7,
type=float,
help='Tags smaller than that are discarded ' +
show_default)
parser.add_argument('-nowait',
dest='nowait',
default=False,
action='store_true',
help='Process all input without waiting for user')
apriltag.add_arguments(parser)
options = parser.parse_args()
if (options.f1 < options.f0):
options.f1 = options.f0
print(options)
det = apriltag.Detector(options)
# For Quad Contour Params Detection (QCP)
#det.tag_detector.contents.qcp.min_side_length = options.min_side_length;
#det.tag_detector.contents.qcp.min_aspect = options.min_aspect;
# Defaults for Quad Threshold Approach
# qtp->max_nmaxima = 10;
# qtp->min_cluster_pixels = 5;
# qtp->max_line_fit_mse = 1.0;
# qtp->critical_rad = 10 * M_PI / 180;
# qtp->deglitch = 0;
# qtp->min_white_black_diff = 15;
qtp = det.tag_detector.contents.qtp
qtp.min_cluster_pixels = 200
qtp.critical_rad = 50.0 / 180 * math.pi
qtp.min_white_black_diff = 30
qtp.deglitch = 0
# ('max_nmaxima', ctypes.c_int),
# ('max_line_fit_mse', ctypes.c_float),
init_gui()
if (options.video_in): # Input is a video
print('Processing video {}'.format(options.video_in))
vidcap = cv2.VideoCapture(options.video_in)
vidcap.set(cv2.CAP_PROP_POS_FRAMES, 0)
nframes = vidcap.get(cv2.CAP_PROP_FRAME_COUNT)
options.f1 = int(min(options.f1, nframes))
win = cv2.namedWindow('tags', cv2.WINDOW_NORMAL)
os.makedirs('tagout', exist_ok=True)
for f in range(options.f0, options.f1 + 1):
filename = "tagout/tagout_{:05d}.png".format(f)
fps = 22
vidcap.set(cv2.CAP_PROP_POS_MSEC, 1000.0 / fps * f)
status, orig = vidcap.read()
# Caution: orig in BGR format by default
if (orig is None):
print('Warning: could not read frame {}'.format(f))
continue
print("Detecting on frame {}, saving to {}".format(f, filename))
detections = do_detect(det, orig)
plot_detections(detections, gax[0], orig)
tagimg = orig.copy()
draw_detections(tagimg, detections, draw_sampling=0)
#tagimg = cv2.cvtColor(tagimg, cv2.COLOR_RGB2BGR);
cv2.imwrite(filename, tagimg)
#print_detections(detections, show_details=False)
plt.draw()
plt.show(block=False)
#_ = input("Stopped at frame {}. Press Enter to continue".format(f))
else: # Input is an image (or several)
print('Processing image(s) {}'.format(options.filenames))
for filename in options.filenames:
if have_cv2:
orig = cv2.imread(filename)
else:
pil_image = Image.open(filename)
orig = numpy.array(pil_image)
#gray = numpy.array(pil_image.convert('L'))
detections = do_detect(det, orig)
plot_detections(detections, gax[0], orig)
tagimg = orig.copy()
draw_detections(tagimg, detections, draw_sampling=0)
#tagimg = cv2.cvtColor(tagimg, cv2.COLOR_RGB2BGR);
cv2.imwrite(filename, tagimg)
plt.show(block=False)
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():
'''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
def apriltag_video(
input_streams=[
'../media/input/single_tag.mp4', '../media/input/multiple_tags.mp4'
], # For default cam use -> [0]
output_stream=False,
display_stream=True,
detection_window_name='AprilTag',
):
'''
Detect AprilTags from video stream.
Args: input_streams [list(int/str)]: Camera index or movie name to run detection algorithm on
output_stream [bool]: Boolean flag to save/not stream annotated with detections
display_stream [bool]: Boolean flag to display/not stream annotated with detections
detection_window_name [str]: Title of displayed (output) tag detection window
'''
parser = ArgumentParser(description='Detect AprilTags from video stream.')
apriltag.add_arguments(parser)
options = parser.parse_args()
'''
Set up a reasonable search path for the apriltag DLL.
Either install the DLL in the appropriate system-wide
location, or specify your own search paths as needed.
'''
detector = apriltag.Detector(options, searchpath=apriltag._get_dll_path())
for stream in input_streams:
video = cv2.VideoCapture(stream)
output = None
if output_stream:
width = int(video.get(cv2.CAP_PROP_FRAME_WIDTH))
height = int(video.get(cv2.CAP_PROP_FRAME_HEIGHT))
fps = int(video.get(cv2.CAP_PROP_FPS))
codec = cv2.VideoWriter_fourcc(*'XVID')
if type(stream) != int:
output_path = '../media/output/' + str(
os.path.split(stream)[1])
output_path = output_path.replace(
str(os.path.splitext(stream)[1]), '.avi')
else:
output_path = '../media/output/' + 'camera_' + str(
stream) + '.avi'
output = cv2.VideoWriter(output_path, codec, fps, (width, height))
while (video.isOpened()):
success, frame = video.read()
if not success:
break
result, overlay = apriltag.detect_tags(
frame,
detector,
camera_params=(3156.71852, 3129.52243, 359.097908, 239.736909),
tag_size=0.0762,
vizualization=3,
verbose=3,
annotation=True)
if output_stream:
output.write(overlay)
if display_stream:
cv2.imshow(detection_window_name, overlay)
if cv2.waitKey(1) & 0xFF == ord(
' '): # Press space bar to terminate
break
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
