def detect(self, image):
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
options = apriltag.DetectorOptions(families="tag36h11")
detector = apriltag.Detector(options)
results = detector.detect(gray)
for r in results:
(ptA, ptB, ptC, ptD) = r.corners
ptB = (int(ptB[0]), int(ptB[1]))
ptC = (int(ptC[0]), int(ptC[1]))
ptD = (int(ptD[0]), int(ptD[1]))
ptA = (int(ptA[0]), int(ptA[1]))
# extract the bounding box (x, y)-coordinates for the AprilTag
# and convert each of the (x, y)-coordinate pairs to integers
# draw the bounding box of the AprilTag detection
cv2.line(image, ptA, ptB, (0, 255, 0), 2)
cv2.line(image, ptB, ptC, (0, 255, 0), 2)
cv2.line(image, ptC, ptD, (0, 255, 0), 2)
cv2.line(image, ptD, ptA, (0, 255, 0), 2)
# draw the center (x, y)-coordinates of the AprilTag
(cX, cY) = (int(r.center[0]), int(r.center[1]))
cv2.circle(image, (cX, cY), 5, (0, 0, 255), -1)
# draw the estimated corner of the paper
(cX_up, cY_up) = self.estimatCorner(results)
cv2.circle(image, (cX_up, cY_up), 9, (255, 0, 0), -1)
(cX_dw, cY_dw) = (int(r.center[0]), int(r.center[1]))
cv2.circle(image, (cX_up, cY_up), 9, (255, 0, 0), -1)
# show the output image after AprilTag detection
return image
def __init__(self, server_):
self.frame = None
self.server = server_
self.tag_name = ""
self.target_name = ""
self.width = int(self.server.video.width)
self.height = int(self.server.video.height)
self.quad_x = int(self.width / 2)
self.quad_y = int(self.height / 2)
self.theta = math.radians(62.2) # width angle of picam
self.psi = math.radians(48.8) # height angle of picam
self.pix_to_meter = 0
self.altitude = 1
options = apriltag.DetectorOptions(families='tag36h11',
border=1,
nthreads=1,
quad_decimate=1.0,
quad_blur=0.0,
refine_edges=True,
refine_decode=True,
refine_pose=True,
debug=False,
quad_contours=True)
self.detector = apriltag.Detector(options)
self.x = None
self.y = None
def __init__(self, args):
# state
self.input_camera = [None, None]
self.april_detector = True
self.rate = 0
self.kin_joints = JointState()
self.kin_joints.name = ["tilt", "lift", "yaw", "pitch"]
self.kin_joints.position = [
-0.10471975803375244, 0.9031959176063538, 0.06871610879898071,
0.14394205808639526
]
self.kin_joints.effort = [0, 0, 0, 1]
# init the robot Kinetic control.
self.robot = miro.interface.PlatformInterface()
# handle april
if "apriltag" not in sys.modules:
raise ValueError("April Tags library not available")
options = apriltag.DetectorOptions( \
families='tag36h11',
border=0,
nthreads=128,
quad_decimate=2.0,
quad_blur=0.0,
refine_edges=False,
refine_decode=False,
refine_pose=False,
debug=False,
quad_contours=True)
self.april_detector = apriltag.Detector(options)
# ROS -> OpenCV converter
self.image_converter = CvBridge()
# robot name
topic_base_name = "/" + os.getenv("MIRO_ROBOT_NAME")
# subscribe
self.sub_caml = rospy.Subscriber(topic_base_name +
"/sensors/caml/compressed",
CompressedImage,
self.callback_caml,
queue_size=1,
tcp_nodelay=True)
#self.sub_camr = rospy.Subscriber(topic_base_name + "/sensors/camr/compressed", CompressedImage, self.callback_camr, queue_size=1, tcp_nodelay=True)
self.sub_kin = rospy.Subscriber(topic_base_name +
"/sensors/kinematic_joints",
JointState,
self.callback_kin,
queue_size=1,
tcp_nodelay=False)
self.kinematic_joints_pub = rospy.Publisher(
topic_base_name + "/control/kinematic_joints",
JointState,
queue_size=1)
def tag_detect(self, data):
# Convert ros gray img to cv2:
cv2_frame = self.bridge.imgmsg_to_cv2(data, "mono8")
# Tag detection:
options = apriltag.DetectorOptions(families="tag36h11")
detector = apriltag.Detector(options)
results = detector.detect(cv2_frame)
if len(results) > 0:
return results[0].tag_id
else:
return -1
def __init__(self, tag_size=0.03, x_length=0.225, y_length=0.14):
self.bridge = CvBridge()
options = apriltag.DetectorOptions(families="tag16h5")
self.detector = apriltag.Detector(options)
# TODO: This needs to map from tag ids to corner locations. Should probably learn it on first image rather than specify
self.corner_ids = {7: 0, 5: 1, 6: 2, 4: 3}
self.tag_size = tag_size
self.x_length = x_length
self.y_length = y_length
self.px_per_m = 5000
xPix = int(self.x_length * self.px_per_m)
yPix = int(self.y_length * self.px_per_m)
self.corner_coords = numpy.array(
[[-self.x_length / 2, -self.y_length / 2, 0],
[self.x_length / 2, -self.y_length / 2, 0],
[-self.x_length / 2, self.y_length / 2, 0],
[self.x_length / 2, self.y_length / 2, 0]])
self.pts_dst = numpy.array([[0, 0], [xPix, 0], [0, yPix], [xPix,
yPix]])
offset = int(self.tag_size / 2 * self.px_per_m)
self.output_shape = (xPix + 2 * offset, yPix + 2 * offset)
self.pts_dst += offset
self.corner_locations = numpy.empty((4, 2), numpy.float32)
self.corner_locations[:] = numpy.nan
self.corner_positions = numpy.empty((4, 3), numpy.float32)
self.corner_positions[:] = numpy.nan
if DEBUG:
self.pc_pub = rospy.Publisher("/test_cloud",
PointCloud2,
queue_size=10)
self.pc_pub2 = rospy.Publisher("/test_cloud2",
PointCloud2,
queue_size=10)
self.tf_broadcaster = tf2_ros.TransformBroadcaster()
self.transformed_workspace_pub = rospy.Publisher(
'transformed_workspace', Image, queue_size=10)
self.camera_params = None
def detect_apriltag(gray, image):
options = apriltag.DetectorOptions(families="tag36h11")
detector = apriltag.Detector(options)
#results = detector.detect(img=gray,True, camera_params=[544.021136,542.307110,308.111905,261.603373], tag_size=0.044)
results = detector.detect(img=gray)
if len(results) > 0:
print("[INFO] {} total AprilTags detected".format(len(results)))
else:
print("No AprilTag Detected")
return
image = np.array(image)
# loop over the AprilTag detection results
for r in results:
# extract the bounding box (x, y)-coordinates for the AprilTag
# and convert each of the (x, y)-coordinate pairs to integers
(ptA, ptB, ptC, ptD) = r.corners
ptB = (int(ptB[0]), int(ptB[1]))
ptC = (int(ptC[0]), int(ptC[1]))
ptD = (int(ptD[0]), int(ptD[1]))
ptA = (int(ptA[0]), int(ptA[1]))
# draw the bounding box of the AprilTag detection
cv2.line(image, ptA, ptB, (0, 255, 0), 2)
cv2.line(image, ptB, ptC, (0, 255, 0), 2)
cv2.line(image, ptC, ptD, (0, 255, 0), 2)
cv2.line(image, ptD, ptA, (0, 255, 0), 2)
# draw the center (x, y)-coordinates of the AprilTag
(cX, cY) = (int(r.center[0]), int(r.center[1]))
cv2.circle(image, (cX, cY), 5, (0, 0, 255), -1)
# draw the tag family on the image
tagFamily = r.tag_family.decode("utf-8")
cv2.putText(image, tagFamily, (ptA[0], ptA[1] - 15),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
#print("[INFO] tag family: {}".format(tagFamily))
M, e1, e2 = detector.detection_pose(
r, [544.021136, 542.307110, 308.111905, 261.603373])
#w:QR code length
w = 4.4
t = [M[0][3] * w, M[1][3] * w, M[2][3] * w]
dist = (t[0]**2 + t[1]**2 + t[2]**2)**0.5
print("[INFO] dist:", dist, " tag pose:", t)
def __init__(self, sys):
node.Node.__init__(self, sys, "detect_april")
options = apriltag.DetectorOptions( \
families='tag16h5',
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(options)
def extract_markers(frame, marker_family, scan_for_inverted_markers=False):
"""
Takes in a RGB color frame and extracts all of the apriltag markers present. Returns a list of markers.
:param frame: The frame to search.
:param marker_family: The marker family to search for.
:param scan_for_inverted_markers: Determines if the image should be checked for markers that are inverted.
:return: {[Marker]} List of markers
"""
log.info('Extracting apriltag markers from camera frame.'
+ (' Checking for inverted markers as well.' if scan_for_inverted_markers else ''))
markers = []
options = apriltag.DetectorOptions(families=marker_family)
detector = apriltag.Detector(options)
gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
gray = Camera.blur_frame(gray, 2)
results = detector.detect(gray)
if scan_for_inverted_markers:
results += detector.detect(Camera.invert_colors(gray))
for r in results:
c0, c1, c2, c3 = r.corners
c0 = np.array([
int(c0[0]),
int(c0[1])
])
c1 = np.array([
int(c1[0]),
int(c1[1])
])
c2 = np.array([
int(c2[0]),
int(c2[1])
])
c3 = np.array([
int(c3[0]),
int(c3[1])
])
fid = str(r.tag_id)
markers.append(
Marker(
fid,
[c0, c1, c2, c3]
)
)
log.info(f"Found {len(markers)} markers: {Marker.get_fids_from_list(markers)}")
return markers
def __init__(self):
self.jdrc = None
self.myCmdvel = None
self.myCamera = None
self.myExtra = None
self.max_speed = 0.07
self.dead_band_x = 0.2 #Real_imagen #Real_relativas 0.4 #Virtual 0.2
self.dead_band_y = 0.15 #Real_imagen #Real_relativas 0.15 #Virtual 0.2
self.dead_band_z = 0.3 #Real_imagen #Real_relativas 0.3 #Virtual 0.2
self.min_z = 2.8
self.dead_band_w = 0.230 #Real_imagen #Real_relativas 0.230 #0.2
self.center = [0, 0]
self.kp = 0.1 #Real_relativas 0.1 #Virtual 0.1
self.kd = 1500 #Real_imagen 0.01 #Real_relativas 0.7 #Virtual 0.03
self.ki = 0.01 #Real_imangen #Real_relativas0.01 #Virtual 0.005
self.kpw = 0.1 #Virtual 0.1
self.kdw = 1500 #Real 0.7 #Virtual 0
self.kiw = 0.01 #Real 0.01 #Virtual 0
self.kpz = 0.4 #Real 0.4 #Virtual 0.1
self.kdz = 1500 #Real 0.7#Virtual 0.01
self.kiz = 0.01 # Real 0.01# Virtual 0.001
self.cycle = 50 #in ms
self.error_xy_anterior = [0, 0]
self.error_z_anterior = 0
self.error_w_anterior = 0
self.vxi = 0
self.vyi = 0
self.vzi = 0
self.vwi = 0
#AprilTags
self.options = apriltag.DetectorOptions()
self.detector = apriltag.Detector(self.options)
self.MARKER_SIZE = 0.28
#Pose
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.pitch = 0.0
#Beacons
self.beacons = [4, 7]
self.beaconCounter = 0
self.switchBeacon = False
self.timeToLand = False
self.connectProxies()
def __init__(self, args):
# state
self.input_camera = [None, None]
self.april_detector = True
# init the robot Kinetic control.
self.robot = miro.interface.PlatformInterface()
# handle april
if "apriltag" not in sys.modules:
raise ValueError("April Tags library not available")
options = apriltag.DetectorOptions( \
families='tag16h5',
border=1,
nthreads=4,
quad_decimate=1.0,
quad_blur=0.0,
refine_edges=True,
refine_decode=False,
refine_pose=False,
debug=True,
quad_contours=True)
self.april_detector = apriltag.Detector(options)
# ROS -> OpenCV converter
self.image_converter = CvBridge()
# robot name
topic_base_name = "/" + os.getenv("MIRO_ROBOT_NAME")
# subscribe
self.sub_caml = rospy.Subscriber(topic_base_name +
"/sensors/caml/compressed",
CompressedImage,
self.callback_caml,
queue_size=1,
tcp_nodelay=True)
self.sub_camr = rospy.Subscriber(topic_base_name +
"/sensors/camr/compressed",
CompressedImage,
self.callback_camr,
queue_size=1,
tcp_nodelay=True)
# report
print "recording from 2 cameras, press CTRL+C to halt..."
def tag_detect(self, data):
# Converter a imagem ros para imagem cv2
cv2_frame = self.bridge.imgmsg_to_cv2(data, "mono8")
# cv2_frame_grey = cv2.cvtColor(cv2_frame, cv2.COLOR_BGR2GRAY)
# Tag detection
options = apriltag.DetectorOptions(families="tag36h11")
detector = apriltag.Detector(options)
results = detector.detect(cv2_frame)
# for r in results:
# (cX, cY) = (int(r.center[0]), int(r.center[1]))
# cv2.circle(cv2_frame, (cX, cY), 5, (0,0,255), -1)
if len(results) > 0:
return results[0].tag_id
# print('[info] tag detectada')
else:
return -1
print('[info] buscando por tags...')
def callback(self, data):
# Converter a imagem ros para imagem cv2
cv2_frame = self.bridge.imgmsg_to_cv2(data, "bgr8")
cv2_frame_grey = cv2.cvtColor(cv2_frame, cv2.COLOR_BGR2GRAY)
# Tag detection
options = apriltag.DetectorOptions(families="tag36h11")
detector = apriltag.Detector(options)
results = detector.detect(cv2_frame_grey)
# for r in results:
# (cX, cY) = (int(r.center[0]), int(r.center[1]))
# cv2.circle(cv2_frame, (cX, cY), 5, (0,0,255), -1)
if len(results) > 0:
self.tag_id_pub.publish(results[0].tag_id)
else:
self.tag_id_pub.publish(-1)
print('[info] buscando por tags...')
def __init__(self):
self.jdrc = None
self.myCmdvel = None
self.myCamera = None
self.max_speed = 5
self.dead_band_x = 0.0 #Real 0.4 #Virtual 0.2
self.dead_band_y = 0.0 #Virtual 0.2
self.dead_band_z = 0.0 #Real 0.4 #Virtual 0.2
self.min_z = 2.4
self.dead_band_w = 0.0 #0.2
self.center = [0, 0]
self.kp = 0.2 #Real 0.05 #Virtual 0.1
self.kd = 0.05 #Real 0.0001 #Virtual 0.03
self.ki = 0.001 #Virtual 0.005
self.kpz = 0.01
self.kdz = 0.05
self.kiz = 0.001
self.kpw = 1
self.kdw = 0
self.kiw = 0
self.cycle = 50 #in ms
self.error_xy_anterior = [0, 0]
self.error_z_anterior = 0
self.error_w_anterior = 0
self.vxi = 0
self.vyi = 0
self.vzi = 0
self.vwi = 0
#AprilTags
self.options = apriltag.DetectorOptions()
self.detector = apriltag.Detector(self.options)
#Pose
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.pitch = 0.0
self.MARKER_SIZE = 0.28
self.connectProxies()
def __init__(self, image):
######################################
# SETTING UP VARIABLES
######################################
# assuming that the image is already grayscale
# self.image = image
self.imageArray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
self.threshold = 70
self.camera_cal_matrix = np.array([[1.11379904e+03, 0.00000000e+00, 5.67239537e+02],
[0.00000000e+00, 1.16158572e+03, 1.04091897e+02],
[0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
options = apriltag.DetectorOptions(families='tag36h11',
border=4,
nthreads=4,
quad_decimate=1.0,
quad_blur=0.0,
refine_edges=True,
refine_decode=True,
refine_pose=True,
debug=True,
quad_contours=False)
self.StringToList = self.getResults()
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 start():
#img = cv2.imread('tag-standard-41h12.png', cv2.IMREAD_GRAYSCALE)
img_dir = input('input image dir: ')
img_name = input('input the image name: ')
img = cv2.imread(img_dir + '/' + img_name, cv2.IMREAD_GRAYSCALE)
options = apriltag.DetectorOptions(families='tagstandard41h12',
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)
detector = apriltag.Detector()
results = detector.detect(img)
print(results)
if len(results) > 0:
print(results[0].tag_family)
print(results[0].center)
print('number of detections: ', len(results))
def __init__(self):
options = apriltag.DetectorOptions(refine_pose=True)
self.detector = apriltag.Detector(options=options)
import apriltag
import cv2
import time
cam = cv2.VideoCapture(1)
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=True,
debug=False,
quad_contours=True)
detector = apriltag.Detector()
for i in range(40):
ret, frame = cam.read()
#cv2.imwrite("her.png", frame)
#img = cv2.imread('her.png', cv2.IMREAD_GRAYSCALE)
img = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
'''scale_percent = 30 # percent of original size
width = int(img.shape[1] * scale_percent / 100)
height = int(img.shape[0] * scale_percent / 100)
dim = (width, height)'''
#img = cv2.resize(img, dim)
result = detector.detect(img)
def apriltag_dection():
global ser
while True:
ret, image = video_capture.read()
# if we are viewing a video and we did not grab a frame,
# then we have reached the end of the video
if image is None:
break
small_frame = cv2.resize(image, (0, 0), fx=frameSize, fy=frameSize)
#print(small_frame.shape)
gray = cv2.cvtColor(small_frame, cv2.COLOR_BGR2GRAY)
#print("[INFO] detecting AprilTags...")
options = apriltag.DetectorOptions(families="tag36h11")
detector = apriltag.Detector(options)
results = detector.detect(gray)
#if there is no AprilTag in the frame, search for it
if not len(results):
print("No AprilTags detected, searching for...")
ser.write("1 Right .2".encode())
print("Right\n");
#x_time = time.time()
#time.sleep(robot_delay)
#start_time = time.time()
print(ser.readline().decode())
print(ser.readline().decode())
#print("Start - X: " + str(start_time-x_time) + "\n")
#ser.write("Stop".encode())
#print("Stop\n");
#middle_time = time.time()
#print(ser.readline().decode())
#print(ser.readline().decode())
#end_time = time.time()
#print("END - MIDDLE: " + str(end_time - middle_time) + "\nMIDDLE - START: " + str(middle_time - start_time))
#time.sleep(robot_delay)
#cv2.imshow("Image", small_frame)
else:
print("[INFO] {} total AprilTags detected".format(len(results)))
# loop over the AprilTag detection results
for r in results:
print("tag_id:" + str(r.tag_id) + "\n")
# extract the bounding box (x, y)-coordinates for the AprilTag
# and convert each of the (x, y)-coordinate pairs to integers
(ptA, ptB, ptC, ptD) = r.corners
ptB = (int(ptB[0]), int(ptB[1]))
ptC = (int(ptC[0]), int(ptC[1]))
ptD = (int(ptD[0]), int(ptD[1]))
ptA = (int(ptA[0]), int(ptA[1]))
(cX, cY) = (int(r.center[0]), int(r.center[1]))
#print the coordinate
#print("Distance A_B" + str(distance(ptA,ptB)))
#print("Distance A_D" + str(distance(ptA,ptD)))
#print("Distance B_C" + str(distance(ptB,ptC)))
#print("Distance C_D" + str(distance(ptC,ptD)))
#compute the distance between webcam and the tag
#limit: the apriltag has to be right in front the camera for the best approximation
bound_length = [distance(ptA,ptB),distance(ptA,ptD),distance(ptB,ptC),distance(ptC,ptD)];
average_bound_length = statistics.mean(bound_length);
print("distance: " + str(4791.1*average_bound_length**-1.042) +" cm")
cv2.putText(small_frame, "distance: " + str(round(4791.1*average_bound_length**-1.042,3)) +" cm", (540,480), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
# draw the bounding box of the AprilTag detection
cv2.line(small_frame, ptA, ptB, (0, 0, 255), 2)
cv2.line(small_frame, ptB, ptC, (0, 0, 255), 2)
cv2.line(small_frame, ptC, ptD, (0, 0, 255), 2)
cv2.line(small_frame, ptD, ptA, (0, 0, 255), 2)
# draw the center (x, y)-coordinates of the AprilTag
cv2.circle(small_frame, (cX, cY), 5, (0, 0, 255), -1)
print("Center: " + str(cX) + ", " + str(cY))
# draw the tag family on the image
tagFamily = r.tag_family.decode("utf-8")
cv2.putText(small_frame, tagFamily, (ptA[0], ptA[1] - 15),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)
print("[INFO] tag family: {}".format(tagFamily))
# show the output image after AprilTag detection
#cv2.imshow("Image", small_frame)
if is_center(cX) == True:
print("Apriltag Centered...")
distance_to_tag = 4791.1*average_bound_length**-1.042;
print(distance_to_tag);
if(distance_to_tag > 60.0):
print("Following the Tag...")
ser.write("1 Forward .5".encode())
print(ser.readline().decode())
print(ser.readline().decode())
#time.sleep(robot_delay)
#print(ser.readline().decode())
#ser.write("Stop".encode())
#print(ser.readline().decode())
#time.sleep(robot_delay)
else:
ser.write("Stop".encode())
print(ser.readline().decode())
print(ser.readline().decode())
else:
print("Apriltag Not Centered..")
if(cX < 350):
print("Shift Left...")
ser.write("1 Left .1".encode())
print(ser.readline().decode())
print(ser.readline().decode())
#time.sleep(robot_delay)
#print(ser.readline().decode())
#ser.write("Stop".encode())
#print(ser.readline().decode())
#time.sleep(robot_delay)
elif(cX > 610):
print("Shift Right...")
ser.write("1 Right .1".encode())
print(ser.readline().decode())
print(ser.readline().decode())
#time.sleep(robot_delay)
#print(ser.readline().decode())
#ser.write("Stop".encode())
#print(ser.readline().decode())
#time.sleep(robot_delay)
else:
ser.write("Stop".encode())
print(ser.readline().decode())
print(ser.readline().decode())
#print(ser.readline().decode())
# Hit 'q' on the keyboard to quit!
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# Release handle to the webcam
video_capture.release()
cv2.destroyAllWindows()
import sys import Adafruit_PCA9685 import time import RPi.GPIO as GPIO GPIO.setmode(GPIO.BCM) GPIO.setwarnings(False) GPIO.setup(18, GPIO.OUT) pwm = Adafruit_PCA9685.PCA9685() pwm.set_pwm_freq(50) cap = cv.VideoCapture(0) cap.set(3, 640) cap.set(4, 480) at_detector = apriltag.Detector(apriltag.DetectorOptions(families='tag36h11')) pwm_x_flag = 0 pwm_y_flag = 0 lastError_x = 0 lastError_y = 0 w_center = 320 h_center = 240 x_dutyCircle = 435 y_dutyCircle = 220 para_x = [lastError_x, x_dutyCircle] para_y = [lastError_y, y_dutyCircle] pwm.set_pwm(9, 0, 435) time.sleep(0.005) pwm.set_pwm(8, 0, 220)
import argparse
import cv2
# Construct the argument parser and parse the arguments
ap = argparse.ArgumentParser()
ap.add_argument("-i", "--image", required=True, help="Path to the input image containing the AprilTag")
args = vars(ap.parse_args())
# Load the input image and convert the image to grayscale
print("[INFO] Loading the image...")
image = cv2.imread(args["image"])
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
# Define the AprilTag detector options and then detect the AprilTags in the input image
print("[INFO] Detecting AprilTags...")
options = apriltag.DetectorOptions(families="tag36h11")
detector = apriltag.Detector(options)
results = detector.detect(gray)
print("[INFO] {} Total AprilTags Detected!".format(len(results)))
# Loop over the AprilTags detection results
for r in results:
# Extract the bounding box (x, y) coordinates for the AprilTag and convert each one of the (x,y) coordinate
# pairs to integers
(ptA, ptB, ptC, ptD) = r.corners
ptB = (int(ptB[0]), int(ptB[1]))
ptC = (int(ptC[0]), int(ptC[1]))
ptD = (int(ptD[0]), int(ptD[1]))
ptA = (int(ptA[0]), int(ptA[1]))
# Draw the bounding box of the AprilTag direction using the extracted (x, y) coordinates
cv2.line(image, ptA, ptB, (0, 255, 0), 2)
def __init__(self):
self.jdrc = None
self.myCmdvel = None
self.myCamera = None
self.myExtra = None
self.maxSpeed = 2 #Real_relativa 0.07 #Cambiar max_speed
#Transitions Variables
self.timeToLand = False
self.timeToApril = False
self.timeToColor = False
#Xml Files
self.xmlFileName = 'calibration_simulated.xml'
#self.xmlFileName = 'calibration.xml'
#Image Center
self.center = [0, 0]
#For PIDs
self.cycle = 50 #in ms
'''Color Beacon Variables'''
#PID Constants
self.kpxColor = 0.005 #0.012 #Virtual 0.005
self.kdxColor = 0.016 #Virtual 0.01
self.kixColor = 0.00004 #Virtual 0.00006
self.kpyColor = 0.0025 #Virtual
self.kdyColor = 0.008 #Virtual
self.kiyColor = 0.00006 #Virtual
self.kpzColor = 0 #Virtual
self.kdzColor = 0 #Virtual
self.kizColor = 0 #Virtual
self.initialize = True
self.stableTime = 0
self.errYPrevColor = 0
self.errXPrevColor = 0
self.yanteriorTot = 0
self.xanteriorTot = 0
#self.x_img=0
#self.y_img=0
#self.landed=True #Cambiar
#self.turnland=0 #Cambiar
#self.numIteracionesOrange=0 #Revisar
#self.numIteracionesGreen=0 #Revisar
#Color Filter
self.minArea = 700 #Gazebo 700
self.PH_max = 180
self.PS_max = 255
self.PV_max = 255
self.PH_min = 0
self.PS_min = 0
self.PV_min = 0
self.PErode = 0
self.PDilate = 0
self.SH_max = 180
self.SS_max = 255
self.SV_max = 255
self.SH_min = 0
self.SS_min = 0
self.SV_min = 0
self.SErode = 0
self.SDilate = 0
self.alpha = 0.6
self.vxiColor = 0
self.vyiColor = 0
self.vziColor = 0
self.xDistanceDeadZone = 0 #Virtual 0
self.yDistanceDeadZone = 0 #Virtual 0
self.xDeadZone = False
self.yDeadZone = False
self.calibration = True
self.timeLimitToLand = 3 #In seconds
'''Apriltag Beacon Variables'''
self.dead_band_x = 0.2 #Real_imagen #Real_relativas 0.4 #Virtual 0.2
self.dead_band_y = 0.15 #Real_imagen #Real_relativas 0.15 #Virtual 0.2
self.dead_band_z = 0.3 #Real_imagen #Real_relativas 0.3 #Virtual 0.2
self.min_z = 2.8
self.dead_band_w = 0.230 #Real_imagen #Real_relativas 0.230 #0.2
self.targetCenter = [0, 0] #CAMBIAR
self.kp = 0.1 #Real_relativas 0.1 #Virtual 0.1
self.kd = 1500 #Real_imagen 0.01 #Real_relativas 0.7 #Virtual 0.03
self.ki = 0.01 #Real_imangen #Real_relativas0.01 #Virtual 0.005
self.kpw = 0.1 #Virtual 0.1
self.kdw = 1500 #Real 0.7 #Virtual 0
self.kiw = 0.01 #Real 0.01 #Virtual 0
self.kpz = 0.4 #Real 0.4 #Virtual 0.1
self.kdz = 1500 #Real 0.7#Virtual 0.01
self.kiz = 0.01 # Real 0.01# Virtual 0.001
self.error_xy_anterior = [0, 0]
self.error_z_anterior = 0
self.error_w_anterior = 0
self.vxi = 0
self.vyi = 0
self.vzi = 0
self.vwi = 0
#AprilTags
self.options = apriltag.DetectorOptions()
self.detector = apriltag.Detector(self.options)
self.MARKER_SIZE = 0.28
#Image Processing
self.cameraMatrix = [187.336, 0, 160, 0, 187.336, 120, 0, 0,
1] #Gazebo simulator
#self.cameraMatrix = [731.37257937739332, 0, 322.35647387552422, 0, 734.23757205576192, 246.30430666269825, 0, 0, 1] #ArDrone 2
#Distorsion Coefficients
self.distCoeffs = np.zeros((5, 1), np.uint8) #Gazebo
#self.distCoeffs = np.array([-0.07304604105914128, 0.50646907582979650, 0.00024443957708413687, 0.00074556540195940392,-1.0762308065763191]) #ArDrone 2
#Pose
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.pitch = 0.0
#Beacons
self.beacons = [4, 7]
self.beaconCounter = 0
self.switchBeacon = False
#Performance monitor
self.minf = 10000
self.maxf = 0
self.avgf = 0
self.listf = [0, 0, 0]
self.c = 0
self.connectProxies()
def get_coordinates(self, cvQueue: Queue):
# To get the coordinate, we rotate on our axis some X number of times to form images that compose a complete
# 360 degree view of our surroundings. We use each image (as long as there are april tags in it) to get a (x,
# z) coordinate value, and then we choose which (x,z) coordinate to return based off of which we deem the
# most correct/reliable (this decision is shown in the code below)
# When turning to search for the desiredTag, we specify time to turn, and time to wait after each semi-turn.
# We do this because we want a stable photo/shot at each
searchingTimeToTurn = 0.5 # seconds
searchingTimeToHalt = 2.0 # seconds
# Note that refine_pose is set to True (takes more work/processing but hopefully gets better coordinates)
options = apriltag.DetectorOptions(
families='tag36h11',
border=1,
nthreads=1,
quad_decimate=1.0,
quad_blur=0.0,
refine_edges=True,
refine_decode=True,
refine_pose=True,
debug=False,
quad_contours=True)
det = apriltag.Detector(options)
# Load camera data
with open('cameraParams.json', 'r') as f:
data = json.load(f)
cameraMatrix = np.array(data['cameraMatrix'], dtype=np.float32)
distCoeffs = np.array(data['distCoeffs'], dtype=np.float32)
# Load world points
world_points = {}
with open('worldPoints.json', 'r') as f:
data = json.load(f)
for k, v in data.items():
world_points[int(k)] = np.array(v, dtype=np.float32).reshape((4, 3, 1))
# Variables for final decision
coordinates_list = []
iterationNumber = 1
numIterations = 10
while True:
# Rotate camera by going left by some amount
self.send_serial_command(Direction.LEFT, b'l')
time.sleep(searchingTimeToTurn)
self.send_serial_command(Direction.STOP, b'h')
time.sleep(searchingTimeToHalt)
# Now lets read the frame (while the robot is halted so that image is clean)
frame = self.vs.read()
if frame is None:
print("ERROR - frame read a NONE")
break
# Use grayscale image for detection
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
res = det.detect(gray)
# Check how many tags we see... if it's 0 then ignore this frame and move on to capturing the next frame
numTagsSeen = len(res)
print("\nNumber of tags seen", numTagsSeen) # TODO remove
if numTagsSeen > 0:
poses = [] # Store poses from each tag to average them over
tagRadiusList = [] # Store tag radius' to determine the largest
for r in res: # Iterate over each tag in the frame
corners = r.corners
tag_id = r.tag_id
corners = np.array(corners, dtype=np.float32).reshape((4, 2, 1))
cornersList = []
for c in corners:
cornersList.append([int(x) for x in c])
cornersList = np.array(cornersList, dtype=np.int32) # Turn into numpy array (openCV wants this)
# Draw circle around tag using its corners & get radius of that tag
((x, y), radius) = cv2.minEnclosingCircle(cornersList)
filteredPtsRadius = [radius]
# Solve pose ((x,z) coordinates)
r, rot, t = cv2.solvePnP(world_points[tag_id], corners, cameraMatrix,
distCoeffs) # get rotation and translation vector using solvePnP
rot_mat, _ = cv2.Rodrigues(rot) # convert to rotation matrix
R = rot_mat.transpose() # Use rotation matrix to get pose = -R * t (matrix mul w/ @)
pose = -R @ t
weight = self.calc_weight(pose, world_points[tag_id][0])
poses.append((pose, weight))
tagRadiusList.append(filteredPtsRadius)
# Done iterating over the tags that're seen in the frame...
# Now get the average pose across the tags and get the largest tag radius that we saw.
# We will store the (x,z) coordinate that we calculate, and we'll also
# store the largest radius for a tag that we've seen in this frame.
avgPose = sum([x * y for x, y in poses]) / sum([x[1] for x in poses])
largestTagRadius = max(tagRadiusList)
coordinates = (avgPose[0][0], avgPose[2][0], largestTagRadius)
print(str(coordinates)) # TODO remove this
coordinates_list.append(coordinates)
# Display frame
cv2.imshow('frame', frame)
# If we've completed our numIterations, then choose the coordinate
# and return (do closing operations too)
if iterationNumber == numIterations:
if len(coordinates_list) > 0:
# TODO 2 things we can try here ...
# 1) The coordinate to return is the one with the smallest z-coordinate
# (which essentially means it's closest to those tags that it used)
# BUT this value seems to vary a lot and I don't think it's reliable
# 2) I have saved the largest radius for a tag seen for each of these
# coordinates, so I can use that (which I bet is more reliable)
# I will go with approach number 2
# coordinateToReturn = min(coordinates_list, key=lambda x: x[1]) # Approach (1)
coordinateToReturn = max(coordinates_list, key=lambda x: x[2]) # Approach (2)
coordinateToReturn = (
int(coordinateToReturn[0]), int(coordinateToReturn[1])) # This stays regardless
else:
coordinateToReturn = (0, -1) # TODO set to some default outside the door
# Cleanup and return
cv2.destroyAllWindows()
print("Value to return:") # TODO remove
print(coordinateToReturn) # TODO remove
return coordinateToReturn
else: # Still have iterations to go, increment the value
iterationNumber += 1
# Q to quit
key = cv2.waitKey(1) & 0xFF
if (key == ord("q")) or (not cvQueue.empty()):
self.send_serial_command(Direction.STOP, b'h')
cv2.destroyAllWindows()
break
def run_cam_thread():
global name
# Initialize some variables
face_locations = []
face_encodings = []
face_names = []
process_this_frame = True
prev_direction = None
while True:
# Grab a single frame of video
ret, frame = video_capture.read()
# Resize frame of video to 1/4 size for faster face recognition processing
small_frame = cv2.resize(frame, (0, 0), fx=frameSize, fy=frameSize)
if (function_index == 1):
# Convert the image from BGR color (which OpenCV uses) to RGB color (which face_recognition uses)
rgb_small_frame = small_frame[:, :, ::-1]
# Only process every other frame of video to save time
if process_this_frame:
# Find all the faces and face encodings in the current frame of video
face_locations = face_recognition.face_locations(
rgb_small_frame)
face_encodings = face_recognition.face_encodings(
rgb_small_frame, face_locations)
name = "Unknown"
face_names = []
for face_encoding in face_encodings:
# See if the face is a match for the known face(s)
matches = face_recognition.compare_faces(
known_face_encodings, face_encoding)
# Or instead, use the known face with the smallest distance to the new face
face_distances = face_recognition.face_distance(
known_face_encodings, face_encoding)
best_match_index = np.argmin(face_distances)
if matches[best_match_index]:
name = known_face_names[best_match_index]
else:
name = "Unknown"
face_names.append(name)
process_this_frame = not process_this_frame
# Display the results
for (top, right, bottom,
left), name in zip(face_locations, face_names):
# Draw a box around the face
cv2.rectangle(small_frame, (left, top), (right, bottom),
(0, 0, 255), 2)
# Draw a label with a name below the face
cv2.rectangle(small_frame, (left, bottom - 35),
(right, bottom), (0, 0, 255), cv2.FILLED)
font = cv2.FONT_HERSHEY_DUPLEX
cv2.putText(small_frame, name, (left + 6, bottom - 6), font,
1.0, (255, 255, 255), 1)
elif (function_index == 2):
print("tracking tag...")
gray = cv2.cvtColor(small_frame, cv2.COLOR_BGR2GRAY)
options = apriltag.DetectorOptions(families="tag36h11")
detector = apriltag.Detector(options)
results = detector.detect(gray)
#if there is no AprilTag in the frame, search for it
if not len(results):
print("No AprilTags detected, searching for...")
if (prev_direction == None or prev_direction == 'Right'):
ser.write("1 Right .2".encode())
print("Right\n")
print(ser.readline().decode())
else:
ser.write("1 Left .2".encode())
print("Left\n")
print(ser.readline().decode())
else:
print("[INFO] {} total AprilTags detected".format(
len(results)))
# loop over the AprilTag detection results
for r in results:
print("tag_id:" + str(r.tag_id) + "\n")
# extract the bounding box (x, y)-coordinates for the AprilTag
# and convert each of the (x, y)-coordinate pairs to integers
(ptA, ptB, ptC, ptD) = r.corners
ptB = (int(ptB[0]), int(ptB[1]))
ptC = (int(ptC[0]), int(ptC[1]))
ptD = (int(ptD[0]), int(ptD[1]))
ptA = (int(ptA[0]), int(ptA[1]))
(cX, cY) = (int(r.center[0]), int(r.center[1]))
#compute the distance between webcam and the tag
#limit: the apriltag has to be right in front the camera for the best approximation
bound_length = [
distance(ptA, ptB),
distance(ptA, ptD),
distance(ptB, ptC),
distance(ptC, ptD)
]
average_bound_length = statistics.mean(bound_length)
print("distance: " +
str(2148.2 * average_bound_length**-1.02) + " cm")
cv2.putText(
small_frame, "distance: " + str(
(round(2148.2 * average_bound_length**-1.02))) +
" cm", (540, 480), cv2.FONT_HERSHEY_SIMPLEX, 1,
(0, 0, 255), 2)
# draw the bounding box of the AprilTag detection
cv2.line(small_frame, ptA, ptB, (0, 0, 255), 2)
cv2.line(small_frame, ptB, ptC, (0, 0, 255), 2)
cv2.line(small_frame, ptC, ptD, (0, 0, 255), 2)
cv2.line(small_frame, ptD, ptA, (0, 0, 255), 2)
# draw the center (x, y)-coordinates of the AprilTag
cv2.circle(small_frame, (cX, cY), 5, (0, 0, 255), -1)
print("Center: " + str(cX) + ", " + str(cY))
# draw the tag family on the image
tagFamily = r.tag_family.decode("utf-8")
cv2.putText(small_frame, tagFamily, (ptA[0], ptA[1] - 15),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)
print("[INFO] tag family: {}".format(tagFamily))
if is_center(cX) == True:
print("Apriltag Centered...")
distance_to_tag = (2148.2 * average_bound_length**-1.02)
print(distance_to_tag)
if (distance_to_tag > 60.0):
print("Following the Tag...")
ser.write("1 Forward .5".encode())
print(ser.readline().decode())
else:
ser.write("Stop".encode())
print(ser.readline().decode())
else:
print("Apriltag Not Centered..")
if (cX < 150):
print("Shift Left...")
ser.write("1 Left .1".encode())
print(ser.readline().decode())
elif (cX > 350):
print("Shift Right...")
ser.write("1 Right .1".encode())
print(ser.readline().decode())
else:
ser.write("Stop".encode())
print(ser.readline().decode())
if cX < 240:
prev_direction = 'Left'
else:
prev_direction = 'Right'
cv2.imshow('Video', small_frame)
# Hit 'q' on the keyboard to quit!
if cv2.waitKey(1) & 0xFF == ord('q'):
break
video_capture.release()
cv2.destroyAllWindows()
def RotateByX(Cy, Cz, thetaX):
rx = thetaX*math.pi/180.0
outY = math.cos(rx)*Cy - math.sin(rx)*Cz
outZ = math.sin(rx)*Cy + math.cos(rx)*Cz
return outY, outZ
cameraParams_Intrinsic = [591,591,321,245] # camera_fx, camera_fy, camera_cx, camera_cy
camera_matrix = np.array(([591.280996, 0, 321.754492],
[0, 591.853914, 245.866165],
[0, 0, 1.0]), dtype=np.double)
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT,480)
cap.set(cv2.CAP_PROP_FRAME_WIDTH,640)
# cap.set(cv2.CAP_PROP_BRIGHTNESS, 5)
tag_detector = apriltag.Detector(apriltag.DetectorOptions(families='tag36h11')) # Build a detector for apriltag
while( cap.isOpened() ):
ret, img = cap.read()
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) # The image must be a grayscale image of type numpy.uint8
key = cv2.waitKey(45)
if key & 0x00FF == 27:
break
tags = tag_detector.detect( gray)
for tag in tags:
cv2.circle(img, tuple(tag.corners[0].astype(int)), 4,(0,0,255), 2) # left-top
cv2.circle(img, tuple(tag.corners[1].astype(int)), 4,(0,0,255), 2) # right-top
cv2.circle(img, tuple(tag.corners[2].astype(int)), 4,(0,0,255), 2) # right-bottom
cv2.circle(img, tuple(tag.corners[3].astype(int)), 4,(0,0,255), 2) # left-bottom
cv2.circle(img, tuple(tag.center.astype(int)), 4,(0,0,255), 2) #center
#print(position_center)
cv2.putText(img,str(tag.tag_id),tuple(tag.center.astype(int)),cv2.FONT_HERSHEY_COMPLEX, 2, (0, 255, 0), 5)
_colorlist = {}
# Open and read in the datafile with the colorlist
with open(args.datafile, 'r') as f:
data = yaml.load(f, Loader=yaml.Loader)
# Organize colorlist so that we can access the color by tag number as index
for obj in data['colorlist']:
_colorlist[obj['tag']] = obj['color']
# print(_colorlist)
# Open Camera
_cam = cv2.VideoCapture(0)
apriltag.DetectorOptions(families='tag36h11')
_detector = apriltag.Detector()
_calibrationRunning = True
_calibrationStep = 0
_data = {}
while _calibrationRunning:
ret_val, img = _cam.read()
grayscale = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Rough guessing of where the screen is based on image differences
if _calibrationStep == 0:
if 'meanBright' not in _data:
_data['meanBright'] = []
def __init__(self, args):
# state
self.input_camera = [None, None]
self.april_detector = True
self.kin_joints = JointState()
self.kin_joints.name = ["tilt", "lift", "yaw", "pitch"]
self.kin_joints.position = [
0.0, math.radians(5),
math.radians(1),
math.radians(7)
]
# init the robot Kinetic control.
self.robot = miro.interface.PlatformInterface()
# handle april
if "apriltag" not in sys.modules:
raise ValueError("April Tags library not available")
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=True,
quad_contours=True)
self.april_detector = apriltag.Detector(options)
# ROS -> OpenCV converter
self.image_converter = CvBridge()
# robot name
topic_base_name = "/" + os.getenv("MIRO_ROBOT_NAME")
# subscribe
self.sub_caml = rospy.Subscriber(topic_base_name +
"/sensors/caml/compressed",
CompressedImage,
self.callback_caml,
queue_size=1,
tcp_nodelay=True)
self.sub_camr = rospy.Subscriber(topic_base_name +
"/sensors/camr/compressed",
CompressedImage,
self.callback_camr,
queue_size=1,
tcp_nodelay=True)
self.sub_lightsens = rospy.Subscriber(topic_base_name +
"/sensors/package",
miro.msg.sensors_package,
self.callback_lightsense,
queue_size=1,
tcp_nodelay=True)
self.kinematic_joints_pub = rospy.Publisher(
topic_base_name + "/control/kinematic_joints",
JointState,
queue_size=0)
# report
print "recording from 2 cameras, press CTRL+C to halt..."
def __init__(self, args):
# state
self.mode = None
self.input_camera = [None, None]
self.april_detector = None
# handle arguments
for arg in args:
if arg in ["show", "record"]:
self.mode = arg
continue
if arg == "--april":
self.april_detector = True
continue
print "argument unrecognised:", arg
exit()
# check mode
if self.mode is None:
error("mode not set")
# handle april
if not self.april_detector is None:
if "apriltag" not in sys.modules:
raise ValueError("April Tags library not available")
self.mode = self.mode.replace("-april", "")
options = apriltag.DetectorOptions( \
families='tag16h5',
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.april_detector = apriltag.Detector(options)
# ROS -> OpenCV converter
self.image_converter = CvBridge()
# robot name
topic_base_name = "/" + os.getenv("MIRO_ROBOT_NAME")
# subscribe
self.sub_caml = rospy.Subscriber(topic_base_name +
"/sensors/caml/compressed",
CompressedImage,
self.callback_caml,
queue_size=1,
tcp_nodelay=True)
self.sub_camr = rospy.Subscriber(topic_base_name +
"/sensors/camr/compressed",
CompressedImage,
self.callback_camr,
queue_size=1,
tcp_nodelay=True)
# report
print "recording from 2 cameras, press CTRL+C to halt..."
def main(argv=None):
if argv is None:
argv = sys.argv
#img_path="./image/"
#img_path = argv[1]
img_path = Path("./image/")
pose_path = Path("./pose")
mask_path = Path("./mask")
distortion = np.array(distortion_v8)
intrinsicMatrix = np.array(intrinsicMatrix_v8)
i_img_succeed = 0
all_files = [
os.path.basename(img_path_)
for i_img, img_path_ in enumerate(img_path.files('*.jpg'))
]
all_files.sort(key=lambda x: int(x[:-4]))
img_num = len(all_files)
for i_img, img_file_name in enumerate(all_files):
img_file = img_path + img_file_name
image_src = np.array(io.imread(img_file, True) * 255, np.uint8)
print("segemet image: %s.. %d/%d" %
(os.path.basename(img_file), i_img + 1, img_num))
image_src = deblur(image_src)
options = apriltag.DetectorOptions(families='tag36h11',
border=1,
nthreads=8,
quad_decimate=1.0,
quad_blur=1,
refine_edges=True,
refine_decode=False,
refine_pose=False,
debug=False,
quad_contours=True)
detector = apriltag.Detector(options)
tags = detector.detect(image_src)
#get camera pose
if len(tags) < 4:
print("Tags detection failed, drop image", i_img)
continue
ret, rvec, tvec = detectionCameraPose(i_img, tags, intrinsicMatrix,
distortion)
#get tag mark area
tag_mask = np.zeros(image_src.shape, np.uint8)
tag_corners = [
tags[0].center, tags[1].center, tags[3].center, tags[2].center
]
area_center_src = (tags[0].center + tags[1].center + \
tags[3].center + tags[2].center) / 4
area = np.array([tag_corners], dtype=np.int32)
#get roi
area_rect = cv2.boundingRect(area)
x = area_rect[0]
y = area_rect[1]
dx = area_rect[2]
dy = area_rect[3]
image_ROI = image_src[y:y + dy, x:x + dx]
area_center_roi = area_center_src - [x, y]
# for tag in tags:
# cv2.circle(image_src, (int(tag.corners[3][0]), int(tag.corners[3][1])),
# radius=5, color=255, thickness=40)
#cv2.circle(
# image_ROI, (int(area_center_roi[0]), int(area_center_roi[1])), radius=5, color=255, thickness=20)
#showimg(image_ROI)
shape0_res = patch_size - image_ROI.shape[0] % patch_size
shape1_res = patch_size - image_ROI.shape[1] % patch_size
# add 4 255 row
image_add = np.zeros(
(image_ROI.shape[0] + shape0_res, image_ROI.shape[1] + shape1_res),
np.uint8)
image_add[:-shape0_res, :-shape1_res] = image_ROI
# for 8x8 patch calculate grecomatrix
image = image_add // 32
image_block = view_as_blocks(image,
block_shape=(patch_size, patch_size))
M = np.shape(image_block)[0]
N = np.shape(image_block)[1]
#print("enter")
pool = []
if _MULTIPLUE_THREAD_:
cores = multiprocessing.cpu_count()
pool = multiprocessing.pool.Pool(processes=cores)
result_contrast = np.zeros(shape=(M, N))
# Single thresh
# for i in range(M):
# for j in range(N):
# result_contrast[i][j]=pool_f(i,j)
for i in range(M):
if _MULTIPLUE_THREAD_:
result_contrast[i] = np.array(
pool.map(pool_f, list(image_block[i]))).flatten()
else:
result_contrast[i] = np.array([
pool_f(block) for block in list(image_block[i])
]).flatten()
if _MULTIPLUE_THREAD_:
pool.close()
pool.join()
#print("finish")
result_contrast *= (255.0 / result_contrast.max())
img_contrast = result_contrast.astype(np.uint8)
#showimg(img_contrast)
# how to set this threshold
from skimage.filters import threshold_minimum
from skimage.filters import threshold_otsu
data = img_contrast.ravel()
data = data[data != 0]
img_gaussianblur = cv2.GaussianBlur(data, (5, 5), 0)
otsu_thresh = threshold_otsu(data)
#otsu_thresh, binary = cv2.threshold(
#img_gaussianblur, 0, 255, cv2.THRESH_BINARY+cv2.THRESH_OTSU)
binary = img_contrast < otsu_thresh * 3 / 5
#binary = cv2.adaptiveThreshold(
# img_contrast, 255, cv2.ADAPTIVE_THRESH_MEAN_C, cv2.THRESH_BINARY, 51, 5)
tag_mask = tag_mask < 128
img_binary = binary.astype(np.uint8) * 255
# find object reagion by finding max countour
c_x = img_binary.shape[0] // 2
c_y = img_binary.shape[1] // 2
# You need to choose 4 or 8 for connectivity type
connectivity_ = 8
# Perform the operation
output = cv2.connectedComponentsWithStats(img_binary,
connectivity=connectivity_,
stats=cv2.CV_32S)
# Get the results
# The first cell is the number of labels
num_labels = output[0]
# The second cell is the label matrix
labels = output[1]
# The third cell is the stat matrix
stats = output[2]
# The fourth cell is the centroid matrix
centroids = output[3]
connected_areas = [[stats[i, cv2.CC_STAT_AREA]]
for i in range(num_labels)]
#find out second largest area whose centroids in center reagion
area_center_downsampled = area_center_roi / patch_size
area_center_distance = np.array(
[[np.linalg.norm(centroids[i] - area_center_downsampled)]
for i in range(num_labels)])
area_is_center = area_center_distance < 20 * patch_size / 32
#rule out outsider by condidering if reagion point inside measurement area
area_downsampled = (area - [x, y]) // patch_size
#insider_point = np.zeros(num_labels)
index_counter = np.zeros(num_labels)
index_interval = np.array(connected_areas) // sample_num
for ly in range(labels.shape[0]):
for lx in range(labels.shape[1]):
label = labels[ly][lx]
if area_is_center[label] == False: continue
if index_counter[label] == index_interval[label]:
index_counter[label] = 0
dist = cv2.pointPolygonTest(area_downsampled, (lx, ly),
True)
if dist < -15:
if show_outlier:
cv2.circle(img_contrast, (lx, ly),
radius=3,
color=255,
thickness=1)
showimg(img_contrast)
area_is_center[label] = False
break
index_counter[label] = index_counter[label] + 1
#nlargest=heapq.nlargest(2, range(len(connected_areas)), key=connected_areas.__getitem__)
largest_label, object_label, findsucceed = findSecondLargeIndexWithMask(
connected_areas, area_is_center)
#TODO: check aera
if largest_label == object_label:
print("Segmentation failed, drop image", i_img)
continue
#showimg(labels)
binary = labels == object_label
binary = binary.repeat(patch_size, axis=0)
binary = binary.repeat(patch_size, axis=1)
#binary = binary & tag_mask
# delete final 4 row
binary = binary[:-shape0_res, :-shape1_res]
img_binary = binary.astype(np.uint8) * 255
#median = cv2.medianBlur(img_binary, 33)
median = img_binary
binary = median > 128
result_img = np.zeros(image_ROI.shape, np.uint8)
result_img[binary] = image_ROI[binary]
mask_src = np.zeros(image_src.shape, np.uint8)
mask_src[y:y + dy, x:x + dx] = binary.astype(int) * 255
# # open and close
# from skimage.morphology import opening, closing, erosion, dilation
# from skimage.morphology import square
# selem = square(30)
# selem_open = square(15)
# # close
# closed = closing(binary, selem)
# # open
# opened = opening(closed, selem_open)
# # close
# closed = closing(opened, selem)
write_pose(pose_path, i_img_succeed, intrinsicMatrix, rvec, tvec)
write_mask(mask_path, i_img_succeed, mask_src)
i_img_succeed = i_img_succeed + 1
if show_img:
#image_ROI = image_add[y:y+dy, x:x+dx]
final_img = np.ones(image_src.shape, np.uint8)
final_img[y:y + dy, x:x + dx] = result_img
plt.figure(figsize=(30, 90))
plt.subplot(2, 2, 1)
plt.imshow(img_contrast, cmap=plt.cm.gray)
plt.title('contrast')
plt.subplot(2, 2, 2)
plt.imshow(labels, cmap=plt.cm.gray)
plt.title('labels')
plt.subplot(2, 2, 3)
image_src = cv2.cvtColor(image_src, cv2.COLOR_BGR2RGB)
plt.imshow(image_src, cmap=plt.cm.gray)
plt.title('Original')
plt.subplot(2, 2, 4)
final_img = cv2.cvtColor(final_img, cv2.COLOR_BGR2RGB)
plt.imshow(final_img, cmap=plt.cm.gray)
plt.title('Result')
plt.show()
stoppoint = 0
stoppoint = stoppoint + 1
def detect_apriltag(gray, image):
global imgCount
options = apriltag.DetectorOptions(families="tag36h11")
detector = apriltag.Detector(options)
#results = detector.detect(img=gray,True, camera_params=[544.021136,542.307110,308.111905,261.603373], tag_size=0.044)
results = detector.detect(img=gray)
if len(results) > 0:
print("[INFO] {} total AprilTags detected".format(len(results)))
else:
print("No AprilTag Detected")
return image, "No AprilTag Detected"
info = []
# loop over the AprilTag detection results
for r in results:
# extract the bounding box (x, y)-coordinates for the AprilTag
# and convert each of the (x, y)-coordinate pairs to integers
(ptA, ptB, ptC, ptD) = r.corners
ptB = (int(ptB[0]), int(ptB[1]))
ptC = (int(ptC[0]), int(ptC[1]))
ptD = (int(ptD[0]), int(ptD[1]))
ptA = (int(ptA[0]), int(ptA[1]))
# draw the bounding box of the AprilTag detection
cv2.line(image, ptA, ptB, (0, 255, 0), 2)
cv2.line(image, ptB, ptC, (0, 255, 0), 2)
cv2.line(image, ptC, ptD, (0, 255, 0), 2)
cv2.line(image, ptD, ptA, (0, 255, 0), 2)
# draw the center (x, y)-coordinates of the AprilTag
(cX, cY) = (int(r.center[0]), int(r.center[1]))
#cv2.circle(image, (cX, cY), 5, (0, 0, 255), -1)
# draw the tag family on the image
tagFamily = r.tag_family.decode("utf-8")
#print("[INFO] tag family: {}".format(tagFamily))
M, e1, e2 = detector.detection_pose(
r, [544.021136, 542.307110, 308.111905, 261.603373])
#w:QR code length
w = 4.4
t = [M[0][3] * w, M[1][3] * w, M[2][3] * w]
dist = (t[0]**2 + t[1]**2 + t[2]**2)**0.5
showStr = "dist:" + str(dist)
cv2.putText(image, showStr, (cX, cY), cv2.FONT_HERSHEY_SIMPLEX, 0.5,
(0, 0, 255), 2)
print("[INFO] dist:", dist, " tag pose:", t)
info.append({"dist": dist, "pose_t": t})
#save some photo
'''
if imgCount<50:
cv2.imwrite("/home/gry/AprilDistImg/"+str(imgCount)+".jpg",image)
imgCount=imgCount+1
print("image saved")
'''
return image, str(info)
