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)