def define_boards(self):
# TODO expand to include all known boards
# load the Dual Zumo tag boards
self.axis_length_inches = 0.0508
hexagon_corners = [
np.array([[-0.023, 0.048,0.044], [0.023, 0.048, 0.044],
[0.023,0.003,0.044], [-0.023, 0.003, 0.044]], dtype=np.float32),
np.array([[0.027, 0.048,0.042], [0.050, 0.048, 0.002],
[0.050, 0.003,0.002], [0.027, 0.003, 0.042]], dtype=np.float32),
np.array([[0.050, 0.048,-0.002], [0.027, 0.048,-0.042],
[0.028, 0.003,-0.042], [0.050, 0.003,-0.002]], dtype=np.float32),
np.array([[0.023, 0.048, -0.044], [-0.023, 0.048, -0.044],
[-0.023,0.003, -0.044], [0.023, 0.003, -0.044]], dtype=np.float32),
np.array([[-0.027, 0.048, -0.042], [-0.050, 0.048, -0.002],
[-0.050,0.003, -0.002], [-0.027, 0.003, -0.042]], dtype=np.float32),
np.array([[-0.049, 0.048, 0.002], [-0.027, 0.048, 0.042],
[-0.028, 0.003, 0.042], [-0.049, 0.003, 0.002]], dtype=np.float32)]
# PORT TAG BOARD
self.port_board_ids = np.array(
[[21], [22], [23], [24], [25], [26]], dtype=np.int32)
self.port_board = aruco.Board_create(hexagon_corners,
aruco.getPredefinedDictionary(
aruco.DICT_6X6_250),
self.port_board_ids)
# STARBOARD TAG BOARD
self.star_board_ids = np.array(
[[27], [28], [29], [30], [31], [32]], dtype=np.int32)
self.star_board = aruco.Board_create(hexagon_corners,
aruco.getPredefinedDictionary(
aruco.DICT_6X6_250),
self.star_board_ids)
self.board_list = [self.port_board, self.star_board]
def _make_aruco_board(markers, dictionary):
"""
Makes a cv2.aruco.board from an array of 5 or 4 3D points plus
marker IDs. First column of markers should be marker ids,
next 15 or 12 columns are 3d coordinates
"""
dictionary = aruco.getPredefinedDictionary(dictionary)
#format of ID first, then 15 or 12 columns
boardshape = markers.shape
if boardshape[0] < 1:
raise ValueError("Marker pattern appears to have no markers")
try:
_ = boardshape[1]
except IndexError:
markers = markers.reshape(1, boardshape[0])
boardshape = markers.shape
if boardshape[1] != 16 and boardshape[1] != 13:
raise ValueError("Marker pattern should have either 5 or 4 3D points")
marker_ids = markers[:, 0].astype('int')
markerpoints = markers[:, 1:13]
if boardshape[1] == 16:
markerpoints = markers[:, 4:16].astype('float32')
return aruco.Board_create(markerpoints, dictionary, marker_ids)
def create_board(cor, a_id):
board_corners = [np.array(cor, dtype=np.float32)]
board_ids = np.array([[a_id]], dtype=np.int32)
board = aruco.Board_create(
board_corners, aruco.getPredefinedDictionary(aruco.DICT_5X5_250),
board_ids)
return board
def __init__(self, n_markers, marker_length, field_length, field_width, barrier_heigth, positions, ball, goal):
markers_board = n_markers - 1 # n_markers includes marker for goal
self.marker_ids = np.arange(1, n_markers)
self.marker_list = [Marker(marker_length, positions[i]) for i in range(markers_board)]
corner_ul = np.array([field_width, 0.0, barrier_heigth])
corner_ur = np.array([field_width, field_length, barrier_heigth])
corner_lr = np.array([0.0, field_length, barrier_heigth])
corner_ll = np.array([0.0, 0.0, barrier_heigth])
self.field_corners = np.array([[corner_ul], [corner_ur], [corner_lr], [corner_ll]])
self.object_points = np.array([self.marker_list[i].corners for i in range(markers_board)], np.float32)
self.ball = ball
self.goal = goal
self.dict = aruco.Dictionary_create(n_markers, 6)
self.parameters = aruco.DetectorParameters_create()
self.board = aruco.Board_create(self.object_points, self.dict, self.marker_ids)
self.visible = False
self.set = False
self.ball_in_goal = False
self.corners = None
self.ids = None
self.rvec = None
self.tvec = None
self.rot_mtx = None
self.inv_rot_mtx = None
self.inv_mtx = None
self.tvec_camera = None
self.roi_points = None
self.radius_max = 250.0
self.radius_min = 15.0
self.tolerance = 0.1
def callback(data):
global theID, thePoint, cam_tf, cam_pub
####for real tello
#camera_matrix = np.array([[921.170702, 0.000000, 459.904354], [
# 0.000000, 919.018377, 351.238301], [0.000000, 0.000000, 1.000000]])
#dist_coeffs = np.array(
# [-0.033458, 0.105152, 0.001256, -0.006647, 0.000000])
####for sim
camera_matrix = np.array([[562, 0.000000, 480.5], [0.000000, 562, 360.5],
[0.000000, 0.000000, 1.000000]])
dist_coeffs = np.array([0, 0, 0, 0, 0])
aruco_dict = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL)
board_corners = [np.array(thePoint, dtype=np.float32)]
board_ids = np.array([theID], dtype=np.int32)
board = aruco.Board_create(
board_corners,
aruco.getPredefinedDictionary(aruco.DICT_ARUCO_ORIGINAL), board_ids)
i = 0
corners = []
ids = np.array([data.id], dtype=np.int32)
corners = np.array([[[data.points[0].x, data.points[0].y],
[data.points[1].x, data.points[1].y],
[data.points[2].x, data.points[2].y],
[data.points[3].x, data.points[3].y]]],
dtype=np.float32)
retval, rvec, tvec = aruco.estimatePoseBoard(corners, ids, board,
camera_matrix, dist_coeffs,
None, None)
if retval == 0:
return 0, 0, 0
a = rvec
b = tvec
rr, _ = cv2.Rodrigues(np.array([a[0][0], a[1][0], a[2][0]]))
tt = np.array([b[0][0], b[1][0], b[2][0] + 0.21])
cam_r = rr.transpose()
cam_t = -cam_r.dot(tt)
cam_x = cam_t[0]
cam_y = cam_t[1]
cam_z = cam_t[2]
cam_q = quaternion_from_matrix(cam_r)
tq = [-0.5, 0.5, 0.5, 0.5]
q3 = qmulq(tq, cam_q)
cam_tf.sendTransform((cam_x, cam_y, cam_z), cam_q, rospy.Time.now(),
"Tello_cam_from_marker" + str(theID), "world")
cam_tf_t.sendTransform((0, 0, 0), (0.5, -0.5, 0.5, 0.5), rospy.Time.now(),
"Tello_cam_from_marker_world" + str(theID),
"Tello_cam_from_marker" + str(theID))
pubmsg = PoseStamped()
pubmsg.pose.position.x = cam_x
pubmsg.pose.position.y = cam_y
pubmsg.pose.position.z = cam_z
pubmsg.pose.orientation.w = q3[3]
pubmsg.pose.orientation.x = q3[0]
pubmsg.pose.orientation.y = q3[1]
pubmsg.pose.orientation.z = q3[2]
pubmsg.header = data.header
pubmsg.header.frame_id = "world"
cam_pub.publish(pubmsg)
def arucoBoard(self):
# Aruco marker length, board height, board width
m = self.aruco_size
h = self.board_height
w = self.board_width
# create objPoints for calibration target
h0 = (0 - h / 2)
hm = (m - h / 2)
h1 = (((h - m) / 2) - h / 2)
h2 = (((h + m) / 2) - h / 2)
h3 = ((h - m) - h / 2)
h4 = (h - h / 2)
w0 = (0 - w / 2)
wm = (m - w / 2)
w1 = (((w - m) / 2) - w / 2)
w2 = (((w + m) / 2) - w / 2)
w3 = ((w - m) - w / 2)
w4 = (w - w / 2)
objPoints = []
objPoints.append(
np.array([[w0, h0, 0], [wm, h0, 0], [wm, hm, 0], [w0, hm, 0]],
dtype=np.float32)) # 0
objPoints.append(
np.array([[w0, h1, 0], [wm, h1, 0], [wm, h2, 0], [w0, h2, 0]],
dtype=np.float32)) # 1
objPoints.append(
np.array([[w0, h3, 0], [wm, h3, 0], [wm, h4, 0], [w0, h4, 0]],
dtype=np.float32)) # 2
objPoints.append(
np.array([[w1, h3, 0], [w2, h3, 0], [w2, h4, 0], [w1, h4, 0]],
dtype=np.float32)) # 3
objPoints.append(
np.array([[w3, h3, 0], [w4, h3, 0], [w4, h4, 0], [w3, h4, 0]],
dtype=np.float32)) # 4
objPoints.append(
np.array([[w3, h1, 0], [w4, h1, 0], [w4, h2, 0], [w3, h2, 0]],
dtype=np.float32)) # 5
objPoints.append(
np.array([[w3, h0, 0], [w4, h0, 0], [w4, hm, 0], [w3, hm, 0]],
dtype=np.float32)) # 6
objPoints.append(
np.array([[w1, h0, 0], [w2, h0, 0], [w2, hm, 0], [w1, hm, 0]],
dtype=np.float32)) # 7
ids = np.linspace(0, 7, 8).astype(np.int32)[:, None]
arucoCornerBoard = aruco.Board_create(objPoints, self.aruco_dict, ids)
return arucoCornerBoard, objPoints
def arucoBoard(m=ARUCO_MARKER_LENGTH, h=0.2884, w=0.5932):
# create objPoints for calibration target
h0 = (0 - h / 2)
hm = (m - h / 2)
h1 = (((h - m) / 2) - h / 2)
h2 = (((h + m) / 2) - h / 2)
h3 = ((h - m) - h / 2)
h4 = (h - h / 2)
w0 = (0 - w / 2)
wm = (m - w / 2)
w1 = (((w - m) / 2) - w / 2)
w2 = (((w + m) / 2) - w / 2)
w3 = ((w - m) - w / 2)
w4 = (w - w / 2)
objPoints = []
objPoints.append(
np.array([[w0, h0, 0], [wm, h0, 0], [wm, hm, 0], [w0, hm, 0]],
dtype=np.float32)) # 0
objPoints.append(
np.array([[w0, h1, 0], [wm, h1, 0], [wm, h2, 0], [w0, h2, 0]],
dtype=np.float32)) # 1
objPoints.append(
np.array([[w0, h3, 0], [wm, h3, 0], [wm, h4, 0], [w0, h4, 0]],
dtype=np.float32)) # 2
objPoints.append(
np.array([[w1, h3, 0], [w2, h3, 0], [w2, h4, 0], [w1, h4, 0]],
dtype=np.float32)) # 3
objPoints.append(
np.array([[w3, h3, 0], [w4, h3, 0], [w4, h4, 0], [w3, h4, 0]],
dtype=np.float32)) # 4
objPoints.append(
np.array([[w3, h1, 0], [w4, h1, 0], [w4, h2, 0], [w3, h2, 0]],
dtype=np.float32)) # 5
objPoints.append(
np.array([[w3, h0, 0], [w4, h0, 0], [w4, hm, 0], [w3, hm, 0]],
dtype=np.float32)) # 6
objPoints.append(
np.array([[w1, h0, 0], [w2, h0, 0], [w2, hm, 0], [w1, hm, 0]],
dtype=np.float32)) # 7
ids = np.linspace(0, 7, 8).astype(np.int32)[:, None]
arucoCornerBoard = aruco.Board_create(objPoints, aruco_dict, ids)
return arucoCornerBoard, objPoints
def scale_tags(board, measured_pattern_width):
"""
We can scale the tag on a board,
which is very useful if you've got the tag
on your mobile phone.
:param: the board to scale
:param measured_pattern_width: Width of the tag in mm
"""
model_pattern_width = min(numpy.ptp([i[:, 0] for i in board.objPoints]),
numpy.ptp([i[:, 1] for i in board.objPoints]))
scale_factor = measured_pattern_width / model_pattern_width
scaled_board = []
for tag in board.objPoints:
tag *= scale_factor
scaled_board.append(tag)
return aruco.Board_create(scaled_board, board.dictionary, board.ids)
def __init__(self, cam_param: CameraParameters,
coordinate_converter: CoordinateConverter):
self.cam_param = cam_param
self.coordinate_converter = coordinate_converter
self.success = False
self.marker_dict = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL)
self.parameters = aruco.DetectorParameters_create()
points = [
np.array([(-10, 10, 0), (-1, 10, 0), (-1, 1, 0), (-10, 1, 0)],
'float32'),
np.array([(1, 10, 0), (10, 10, 0), (10, 1, 0), (1, 1, 0)],
'float32'),
np.array([(1, -1, 0), (10, -1, 0), (10, -10, 0), (1, -10, 0)],
'float32'),
np.array([(-10, 1, 0), (-1, -1, 0), (-1, -10, 0), (-10, -10, 0)],
'float32')
]
ids = np.array([[2], [23], [25], [103]])
self.board = aruco.Board_create(points, self.marker_dict, ids)
def single_tag_board(tag_size,
marker_id,
dictionary=aruco.getPredefinedDictionary(
aruco.DICT_ARUCO_ORIGINAL)):
"""
Create a board consisting of a single ArUco tag
:param: tag size in mm
:param: marker id
:param: dictionary to use
"""
tag = numpy.array([[
-tag_size / 2.0, -tag_size / 2.0, 0., tag_size / 2.0, -tag_size / 2.0,
0., tag_size / 2.0, tag_size / 2.0, 0., -tag_size / 2.0,
tag_size / 2.0, 0.
]],
dtype=numpy.float32)
marker_ids = numpy.array([marker_id])
return aruco.Board_create(tag, dictionary, marker_ids)
def __init__(self, cam_param: calibration):
self.cam_param = cam_param
self.success = False
self.marker_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)
self.parameters = aruco.DetectorParameters_create()
# top left top right bot right bot left
points = dodecahedron_aruco_points()
ids = np.array([[0], [1], [2], [3], [4], [5], [6], [7], [8], [9], [10],
[11]])
self.board = aruco.Board_create(points, self.marker_dict, ids)
self.tvec_pencil = pencil_tip_from_length_mm(PENCIL_LENGTH)
camera_to_world = transform.Transform.from_parameters(
np.asscalar(self.cam_param.tvecs[0]),
np.asscalar(self.cam_param.tvecs[1]),
np.asscalar(self.cam_param.tvecs[2]),
np.asscalar(self.cam_param.rvecs[0]),
np.asscalar(self.cam_param.rvecs[1]),
np.asscalar(self.cam_param.rvecs[2]))
self.world_to_camera = camera_to_world.inverse()
tp = self.tvec_pencil
self.stylus_to_tip = transform.Transform.from_parameters(
tp[0], tp[1], tp[2], 0, 0, 0)
def get_board_dictionary():
# create empty dictionary
board_dict = dict()
# corner locations in meters
# small 0.046 inch dual zumo hexagon
hexagon_corners = [
np.array([[-0.023, 0.048, 0.044], [0.023, 0.048, 0.044],
[0.023, 0.003, 0.044], [-0.023, 0.003, 0.044]],
dtype=np.float32),
np.array([[0.027, 0.048, 0.042], [0.050, 0.048, 0.002],
[0.050, 0.003, 0.002], [0.027, 0.003, 0.042]],
dtype=np.float32),
np.array([[0.050, 0.048, -0.002], [0.027, 0.048, -0.042],
[0.028, 0.003, -0.042], [0.050, 0.003, -0.002]],
dtype=np.float32),
np.array([[0.023, 0.048, -0.044], [-0.023, 0.048, -0.044],
[-0.023, 0.003, -0.044], [0.023, 0.003, -0.044]],
dtype=np.float32),
np.array([[-0.027, 0.048, -0.042], [-0.050, 0.048, -0.002],
[-0.050, 0.003, -0.002], [-0.027, 0.003, -0.042]],
dtype=np.float32),
np.array([[-0.049, 0.048, 0.002], [-0.027, 0.048, 0.042],
[-0.028, 0.003, 0.042], [-0.049, 0.003, 0.002]],
dtype=np.float32)
]
# 2 inch zumo octagon
octagon_two_in_corners = [
np.array([[-0.020, 0.046, 0.061], [0.020, 0.046, 0.061],
[0.020, 0.005, 0.061], [-0.020, 0.005, 0.061]],
dtype=np.float32),
np.array([[0.029, 0.046, 0.058], [0.058, 0.046, 0.029],
[0.058, 0.005, 0.029], [0.029, 0.005, 0.058]],
dtype=np.float32),
np.array([[0.061, 0.046, 0.020], [0.061, 0.046, -0.020],
[0.061, 0.005, -0.020], [0.061, 0.005, 0.020]],
dtype=np.float32),
np.array([[0.058, 0.046, -0.029], [0.029, 0.046, -0.058],
[0.029, 0.005, -0.058], [0.058, 0.005, -0.029]],
dtype=np.float32),
np.array([[0.020, 0.046, -0.061], [-0.020, 0.046, -0.061],
[-0.020, 0.005, -0.061], [0.020, 0.005, -0.061]],
dtype=np.float32),
np.array([[-0.029, 0.046, -0.058], [-0.058, 0.046, -0.029],
[-0.058, 0.005, -0.029], [-0.029, 0.005, -0.058]],
dtype=np.float32),
np.array([[-0.061, 0.046, -0.020], [-0.061, 0.046, 0.020],
[-0.061, 0.005, 0.020], [-0.061, 0.005, -0.020]],
dtype=np.float32),
np.array([[-0.058, 0.046, 0.029], [-0.029, 0.046, 0.058],
[-0.029, 0.005, 0.058], [-0.058, 0.005, 0.029]],
dtype=np.float32)
]
# 2.735 in square face octagonal prism
# 2.3 in corner tag centered on faces
octagon_two_three_in_corners = [
np.array([[-0.029, 0.064, 0.084], [0.030, 0.064, 0.084],
[0.030, 0.006, 0.084], [-0.029, 0.006, 0.084]],
dtype=np.float32),
np.array([[0.039, 0.064, 0.080], [0.080, 0.064, 0.038],
[0.080, 0.006, 0.038], [0.039, 0.006, 0.080]],
dtype=np.float32),
np.array([[0.084, 0.064, 0.029], [0.084, 0.064, -0.030],
[0.084, 0.006, -0.030], [0.084, 0.006, 0.029]],
dtype=np.float32),
np.array([[0.080, 0.064, -0.039], [0.038, 0.064, -0.080],
[0.038, 0.006, -0.080], [0.080, 0.006, -0.039]],
dtype=np.float32),
np.array([[0.029, 0.064, -0.084], [-0.030, 0.064, -0.084],
[-0.030, 0.006, -0.084], [0.029, 0.006, -0.084]],
dtype=np.float32),
np.array([[-0.039, 0.064, -0.080], [-0.080, 0.064, -0.038],
[-0.080, 0.006, -0.038], [-0.039, 0.006, -0.080]],
dtype=np.float32),
np.array([[-0.084, 0.064, -0.029], [-0.084, 0.064, 0.030],
[-0.084, 0.006, 0.030], [-0.084, 0.006, -0.029]],
dtype=np.float32),
np.array([[-0.080, 0.064, 0.039], [-0.038, 0.064, 0.080],
[-0.038, 0.006, 0.080], [-0.080, 0.006, 0.039]],
dtype=np.float32)
]
def add_board(name, ids, board_description):
board_ids = np.array(ids, dtype=np.int32)
board_dict[name] = aruco.Board_create(
board_description,
aruco.getPredefinedDictionary(aruco.DICT_6X6_250), board_ids)
add_board("calumet_board",
[[66], [67], [68], [69], [70], [71], [72], [73]],
octagon_two_three_in_corners)
add_board("danube_board", [[74], [75], [76], [77], [78], [79], [80], [81]],
octagon_two_three_in_corners)
add_board("egypt_board", [[82], [83], [84], [85], [86], [87], [88], [89]],
octagon_two_three_in_corners)
add_board("french_board", [[90], [91], [92], [93], [94], [95], [96], [97]],
octagon_two_three_in_corners)
add_board("hoosic_board",
[[98], [99], [100], [101], [102], [103], [104], [105]],
octagon_two_three_in_corners)
add_board("port_board", [[21], [22], [23], [24], [25], [26], [27], [28]],
octagon_two_three_in_corners)
add_board("star_board", [[29], [30], [31], [32], [33], [34], [35], [36]],
octagon_two_three_in_corners)
add_board("ivy_board",
[[114], [115], [116], [117], [118], [119], [120], [121]],
octagon_two_three_in_corners)
add_board("jiffy_board",
[[122], [123], [124], [125], [126], [127], [128], [129]],
octagon_two_three_in_corners)
add_board("kappa_board",
[[130], [131], [132], [133], [134], [135], [136], [137]],
octagon_two_three_in_corners)
add_board("luna_board",
[[138], [139], [140], [141], [142], [143], [144], [145]],
octagon_two_three_in_corners)
add_board("mars_board",
[[146], [147], [148], [149], [150], [151], [152], [153]],
octagon_two_three_in_corners)
# ZUMO BRAVO TAG BOARD
zumo_bravo_board_ids = np.array(
[[58], [59], [60], [61], [62], [63], [64], [65]], dtype=np.int32)
zumo_bravo_board = aruco.Board_create(
octagon_two_in_corners,
aruco.getPredefinedDictionary(aruco.DICT_6X6_250),
zumo_bravo_board_ids)
board_dict["bravo_board"] = zumo_bravo_board
# ZUMO ALPHA TAG BOARD
zumo_alpha_board_ids = np.array(
[[50], [51], [52], [53], [54], [55], [56], [57]], dtype=np.int32)
zumo_alpha_board = aruco.Board_create(
octagon_two_in_corners,
aruco.getPredefinedDictionary(aruco.DICT_6X6_250),
zumo_alpha_board_ids)
board_dict["alpha_board"] = zumo_alpha_board
return board_dict
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
# length from the generated markers
# TODO maker a configuration file
axis_length_inches = 0.0508
hexagon_corners = [
np.array([[-0.023,0.048,0.044], [0.023,0.048,0.044], [0.023,0.003,0.044], [-0.023,0.003,0.044]] ,dtype=np.float32),
np.array([[0.027,0.048,0.042], [0.050,0.048,0.002], [0.050,0.003,0.002], [0.027,0.003,0.042]] ,dtype=np.float32),
np.array([[0.050,0.048,-0.002], [0.027,0.048,-0.042], [0.028,0.003,-0.042], [0.050,0.003,-0.002]] ,dtype=np.float32),
np.array([[0.023,0.048,-0.044], [-0.023,0.048,-0.044],[-0.023,0.003,-0.044],[0.023,0.003,-0.044]] ,dtype=np.float32),
np.array([[-0.027,0.048,-0.042],[-0.050,0.048,-0.002],[-0.050,0.003,-0.002],[-0.027,0.003,-0.042]],dtype=np.float32),
np.array([[-0.049,0.048,0.002], [-0.027,0.048,0.042], [-0.028,0.003,0.042], [-0.049,0.003,0.002]] ,dtype=np.float32)]
port_board_ids = np.array( [[21],[22],[23],[24],[25],[26]], dtype=np.int32)
port_board = aruco.Board_create( hexagon_corners,
aruco.getPredefinedDictionary(aruco.DICT_6X6_250),
port_board_ids )
star_board_ids = np.array( [[27],[28],[29],[30],[31],[32]], dtype=np.int32)
star_board = aruco.Board_create( hexagon_corners,
aruco.getPredefinedDictionary(aruco.DICT_6X6_250),
star_board_ids )
# read the cameraParameters.xml file generated by
# opencv_interactive-calibration
camera_reader = cv2.FileStorage()
camera_reader.open("cameraParameters.xml",cv2.FileStorage_READ)
# camera configurations
camera_matrix = read_node_matrix( camera_reader, "cameraMatrix" )
dist_coeffs = read_node_matrix( camera_reader, "dist_coeffs" )
def run(self):
global last_frame,cam_pub
while 1:
while self.input_queue.qsize() == 0:
pass
msg = self.input_queue.get()
if msg[1]<last_frame:
continue
else:
last_frame=msg[1]
img = ROSdata2CV(msg[0])
corners, ids, img = detect(img)
if ids.__str__() == "None":
continue
for i in range(len(ids)):
if ids[i] >= 20:
continue
a = Marker()
a.id = ids[i]
a.header = msg[0].header
a.points = [Point() for k in range(4)]
for j in range(4):
a.points[j].x = corners[i][0][j][0]
a.points[j].y = corners[i][0][j][1]
a.points[j].z = 1
self.output_queue.put(a)
camera_matrix = np.array([[562, 0.000000, 480.5], [0.000000, 562, 360.5], [0.000000, 0.000000, 1.000000]])
dist_coeffs = np.array([0, 0, 0, 0, 0])
aruco_dict = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL)
board_corners = [np.array([[0,-0.5,0.3],[0,-0.3,0.3],[0,-0.3,0.1],[0,-0.5,0.1]], dtype=np.float32),
np.array([[0,-0.5,0.7],[0,-0.3,0.7],[0,-0.3,0.5],[0,-0.5,0.5]], dtype=np.float32),
np.array([[0,-0.5,1.1],[0,-0.3,1.1],[0,-0.3,0.9],[0,-0.5,0.9]], dtype=np.float32),
np.array([[0,-0.1,0.9],[0,0.1,0.9],[0,0.1,0.7],[0,-0.1,0.7]], dtype=np.float32),
np.array([[0,0.3,0.3],[0,0.5,0.3],[0,0.5,0.1],[0,0.3,0.1]], dtype=np.float32),
np.array([[0,0.3,0.7],[0,0.5,0.7],[0,0.5,0.5],[0,0.3,0.5]], dtype=np.float32),
np.array([[0,0.3,1.1],[0,0.5,1.1],[0,0.5,0.9],[0,0.3,0.9]], dtype=np.float32)]
board_ids = np.array([0,1,2,3,4,5,6], dtype=np.int32)
board = aruco.Board_create(board_corners,
aruco.getPredefinedDictionary(
aruco.DICT_ARUCO_ORIGINAL),
board_ids)
retval, rvec, tvec = aruco.estimatePoseBoard(corners,ids, board, camera_matrix,dist_coeffs)
if retval == 0:
continue
a=rvec
b=tvec
rr, _ = cv2.Rodrigues(np.array([a[0][0], a[1][0], a[2][0]]))
tt = np.array([b[0][0], b[1][0], b[2][0]+0.21])
cam_r = rr.transpose()
cam_t = -cam_r.dot(tt)
cam_x = cam_t[0]
cam_y = cam_t[1]
cam_z = cam_t[2]
cam_q=quaternion_from_matrix(cam_r)
tq=[-0.5, 0.5, 0.5, 0.5]
q3=qmulq(tq,cam_q)
pubmsg=PoseStamped()
pubmsg.pose.position.x=cam_x
pubmsg.pose.position.y=cam_y
pubmsg.pose.position.z=cam_z
pubmsg.pose.orientation.w=q3[3]
pubmsg.pose.orientation.x=q3[0]
pubmsg.pose.orientation.y=q3[1]
pubmsg.pose.orientation.z=q3[2]
pubmsg.header.frame_id="world"
cam_pub.publish(pubmsg)
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_100 )
parameters = aruco.DetectorParameters_create()
parameters.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_APRILTAG
markerWidth = 0.045 # This value is in meters
# Define Aruco board, which can be any 3D shape. See helper CAD file @ https://cad.onshape.com/documents/d51fdec31f121f572b802b11/w/83fac6aaee78bdc978fd804d/e/8ae3ae505e4af3c7402b131a
board_ids = np.array([[94], [95], [96], [97], [98], [99]], dtype=np.int32)
board_corners = [
np.array([[-0.022, 0.023, 0.03], [ 0.023, 0.022, 0.03], [ 0.023,-0.023, 0.03], [-0.022,-0.023, 0.03]], dtype=np.float32),
np.array([[-0.022,-0.03, 0.022], [ 0.023,-0.03, 0.022], [ 0.022,-0.03, -0.022], [-0.022,-0.03, -0.022]], dtype=np.float32),
np.array([[-0.03, -0.023, 0.022], [-0.03, -0.022,-0.023], [-0.03, 0.023,-0.022], [-0.03, 0.023, 0.023]], dtype=np.float32),
np.array([[-0.022,-0.022,-0.03], [ 0.023,-0.023,-0.03], [ 0.023, 0.023,-0.03], [-0.022, 0.023,-0.03]], dtype=np.float32),
np.array([[ 0.03, -0.023,-0.022], [ 0.03, -0.023, 0.023], [ 0.03, 0.023, 0.022], [ 0.03, 0.022,-0.023]], dtype=np.float32),
np.array([[-0.022, 0.03, -0.023], [ 0.023, 0.03, -0.022], [ 0.023, 0.03, 0.023], [-0.022, 0.03, 0.022]], dtype=np.float32)
]
board = aruco.Board_create( board_corners, aruco_dict, board_ids )
# Start the Leap Capture Thread
leap = leapuvc.leapImageThread(resolution=(640, 480))
leap.start()
leap.setExposure(2000)
leap.setGain(20)
# Capture images until 'q' is pressed
while((not (cv2.waitKey(1) & 0xFF == ord('q'))) and leap.running):
newFrame, leftRightImage = leap.read()
if(newFrame):
for i, cam in enumerate(leap.cameras):
# Only track in left camera for speed
if(cam == 'right'):
break
np.array([[0.2, 0.25, 0], [0.2, 0.45, 0], [0.4, 0.45, 0], [0.4, 0.25, 0]],
dtype=np.float32),
np.array(
[[0, 0.25, 0.95], [0, 0.45, 0.95], [0, 0.45, 0.75], [0, 0.25, 0.75]],
dtype=np.float32),
np.array([[0.05, 0.5, 0.148], [0.05, 0.7, 0.148], [0.19142, 0.7, 0.0071],
[0.19142, 0.5, 0.0071]],
dtype=np.float32),
np.array([[0, 0.5, 0.7], [0, 0.7, 0.7], [0, 0.7, 0.5], [0, 0.5, 0.5]],
dtype=np.float32),
np.array([[0, 0.5, 1.2], [0, 0.7, 1.2], [0, 0.7, 1], [0, 0.5, 1]],
dtype=np.float32)
]
board_ids = np.array([[0], [6], [7], [4], [5], [1], [2], [3]], dtype=np.int32)
board = aruco.Board_create(board_corners,
aruco.getPredefinedDictionary(aruco.DICT_5X5_250),
board_ids)
def find_aruco(img):
frame = img
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
gus = cv2.GaussianBlur(gray, (11, 11), 0)
corners, ids, rejectedImgPoints = aruco.detectMarkers(gus, aruco_dict)
#frame_markers = aruco.drawDetectedMarkers(frame.copy(), corners, ids)
retval, rvec, tvec = aruco.estimatePoseBoard(corners, ids, board,
camera_matrix, dist_coeffs,
None, None)
# print(rvec)
# print(tvec)
if not retval:
# objPoints = np.array([[[[-sd/2., sp/2.+sd, ht]], [[sd/2., sp/2.+sd, ht]], [[sd/2., sp/2., ht]], [[-sd/2., sp/2., ht]]],
# [[[-sd/2., -sp/2, ht]], [[sd/2., -sp/2., ht]], [[sd/2., -(sp/2.+sd), ht]], [[-sd/2., -(sp/2.+sd), ht]]]], dtype=np.float32)
cam = cv2.VideoCapture(0)
cam.set(cv2.CAP_PROP_FRAME_WIDTH, FRAME_W)
cam.set(cv2.CAP_PROP_FRAME_HEIGHT, FRAME_H)
cam.set(cv2.CAP_PROP_FPS, FPS)
arudict = aruco.custom_dictionary(8, 3)
logitech_cam_matrix = np.array([6.6419013281274511e+02, 0., 3.3006505461506646e+02, 0.,
6.6587003740196826e+02, 2.3030870519881907e+02, 0., 0., 1.])
logitech_cam_matrix = logitech_cam_matrix.reshape((3, 3))
logitech_dist_coeffs = np.array([2.0806786957819065e-01, -3.6839605435678208e-01,
-1.2078578161331370e-02, 6.4294407773653481e-03,
4.2723310854154123e-01])
car_board0 = aruco.Board_create(objPoints, arudict, np.arange(0, 4))
car_board1 = aruco.Board_create(objPoints, arudict, np.arange(4, 8))
while True:
ret, frame = cam.read()
corners, ids, rejected = aruco.detectMarkers(frame, arudict)
corners, ids, rejected, recovered = aruco.refineDetectedMarkers(frame, car_board0, corners, ids, rejected,
logitech_cam_matrix, logitech_dist_coeffs)
corners, ids, rejected, recovered = aruco.refineDetectedMarkers(frame, car_board1, corners, ids, rejected,
logitech_cam_matrix, logitech_dist_coeffs)
aruco.drawDetectedMarkers(frame, corners, ids)
# rvecs, tvecs = aruco.estimatePoseSingleMarkers(corners, sd, camera_matrix, dist_coeffs)
N0, rvec0, tvec0 = aruco.estimatePoseBoard(corners, ids, car_board0, logitech_cam_matrix, logitech_dist_coeffs)
N1, rvec1, tvec1 = aruco.estimatePoseBoard(corners, ids, car_board1, logitech_cam_matrix, logitech_dist_coeffs)
if N0:
def add_board(name, ids, board_description):
board_ids = np.array(ids, dtype=np.int32)
board_dict[name] = aruco.Board_create(
board_description,
aruco.getPredefinedDictionary(aruco.DICT_6X6_250), board_ids)
def create_car_board(idx):
return aruco.Board_create(objPoints, arudict, np.arange(0, 4) + idx * 4)
