コード例 #1
0
def aruco_read(img_copy):
    """
	Função que processa uma imagem, buscando os QR codes.
	Retorna:
		img_copy: copia da imagem da camera do robo, com
        o contorno dos QRcodes desenhados por cima, e com os IDs dos 
        QRcodes sobrescritos na imagem.

        ids: lista dos IDs dos QRcodes, que a funcao esta
        detectando na imagem.
	"""

    aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_6X6_250)
    # parameters  = aruco.DetectorParameters_create()
    # parameters.minDistanceToBorder = 0
    # parameters.adaptiveThreshWinSizeMax = 1000
    gray = cv2.cvtColor(img_copy, cv2.COLOR_BGR2GRAY)
    corners, ids, rejectedImgPoints = aruco.detectMarkers(
        gray, aruco_dict)  #, parameters=parameters)
    # if ids is None:
    #     return None
    # for i in range(len(ids)):
    #     print('ID: {}'.format(ids[i]))

    #     for c in corners[i]:
    #         for canto in c:
    #             print("Corner {}".format(canto))
    aruco.drawDetectedMarkers(img_copy, corners, ids)

    return img_copy, ids
コード例 #2
0
def detect_Aruco(
        img):  #returns the detected aruco list dictionary with id: corners
    aruco_list = {}
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    aruco_dict = aruco.getPredefinedDictionary(
        aruco.DICT_4X4_1000
    )  #aruco.Dictionary_get(aruco.DICT_4X4_50)   #creating aruco_dict with 5x5 bits with max 250 ids..so ids ranges from 0-249
    print(aruco_dict)
    parameters = aruco.DetectorParameters_create(
    )  #refer opencv page for clarification
    #lists of ids and the corners beloning to each id
    print(parameters)
    corners, ids, _ = aruco.detectMarkers(gray,
                                          aruco_dict,
                                          parameters=parameters)
    #corners is the list of corners(numpy array) of the detected markers. For each marker, its four corners are returned in their original order (which is clockwise starting with top left). So, the first corner is the top left corner, followed by the top right, bottom right and bottom left.
    # print len(corners), corners, ids
    print(corners)
    print(len(corners))
    gray = aruco.drawDetectedMarkers(gray, corners, ids)
    # cv2.imshow('frame',gray)
    #print (type(corners[0]))
    if len(corners):  #returns no of arucos
        #print (len(corners))
        #print (len(ids))
        for k in range(len(corners)):
            temp_1 = corners[k]
            temp_1 = temp_1[0]
            temp_2 = ids[k]
            temp_2 = temp_2[0]
            aruco_list[temp_2] = temp_1
        return aruco_list
コード例 #3
0
def example_calibrate_folder(folder_in, folder_out,marker_length_mm = 3.75, marker_space_mm = 0.5,dct = aruco.DICT_6X6_1000):
    tools_IO.remove_files(folder_out)

    num_cols, num_rows = 4, 5
    board = aruco.GridBoard_create(num_cols, num_rows, marker_length_mm, marker_space_mm,aruco.getPredefinedDictionary(dct))
    filenames = numpy.unique(tools_IO.get_filenames(folder_in, '*.jpg,*.png'))[:3]

    counter, corners_list, id_list, first = [], [], [], True
    for filename_in in filenames:
        base_name, ext = filename_in.split('/')[-1].split('.')[0], filename_in.split('/')[-1].split('.')[1]
        image = cv2.imread(folder_in + filename_in)
        img_gray = cv2.cvtColor(image,cv2.COLOR_RGB2GRAY)
        corners, ids, rejectedImgPoints = aruco.detectMarkers(img_gray, aruco.getPredefinedDictionary(dct))
        if first == True:
            corners_list = corners
            id_list = ids
            first = False
        else:
            corners_list = numpy.vstack((corners_list, corners))
            id_list = numpy.vstack((id_list,ids))
        counter.append(len(ids))

        image_temp = tools_image.desaturate(image.copy())
        aruco.drawDetectedMarkers(image_temp, corners)
        cv2.imwrite(folder_out+base_name+'.png',image_temp)
        print(base_name)


    counter = numpy.array(counter)
    ret, camera_matrix, dist, rvecs, tvecs = aruco.calibrateCameraAruco(corners_list, id_list, counter, board, img_gray.shape, None, None )

    print(camera_matrix)

    return
コード例 #4
0
    def __init__(self):
        global take_off
        # --- Get the camera calibration path
        self.twist = Twist()

        self.workspace_path = '/home/vishal/vishal_testing'
        self.camera_matrix = np.loadtxt(
            self.workspace_path + '/drone-simulation-ws/src/drone_control/src/cameraMatrix.txt', delimiter=',')
        self.camera_distortion = np.loadtxt(
            self.workspace_path + '/drone-simulation-ws/src/drone_control/src/cameraDistortion.txt', delimiter=',')

        # --- 180 deg rotation matrix around the x axis
        self.r_flip = np.zeros((3, 3), dtype=np.float32)
        self.r_flip[0, 0] = 1.0
        self.r_flip[1, 1] = -1.0
        self.r_flip[2, 2] = -1.0

        # --- Define the aruco dictionary
        self.aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_ARUCO_ORIGINAL)
        self.parameters = aruco.DetectorParameters_create()

        # --- Capture the camera video (this may also be a video or a picture)
        self.capture = cv_bridge.CvBridge()

        # -- Font for the text in the image
        self.font = cv2.FONT_HERSHEY_PLAIN

        self.height = 0
        self.downward = 0
        self.upword = 0

        self.height_sub = rospy.Subscriber('/sonar_height', Range, callback=self.height_callback)
        self.camera_image_sub = rospy.Subscriber('/bottom/camera/image', Image, queue_size=5, callback=self.main)
        self.drone_status_pub = rospy.Publisher('land', String, queue_size=1)
        self.velocity_command_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
コード例 #5
0
def startWebCam(cameraMatrix, distCoeffs,arucoSquareDimension):
    

  
    markerCorners=[]

    #markerCorners=[]
    markerDictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
    
    cap = cv2.VideoCapture(cam_1_no)

    while(1):
        _,frame = cap.read()
        frame,new_matrix_1,_=undistortImage(frame,cam_matrix_1,distortion_coefficients_1)
        markerCorners,markerID, rejectedCandidates= aruco.detectMarkers(frame,markerDictionary)
       
       
        r_vect,t_vect, _ =aruco.estimatePoseSingleMarkers(markerCorners,arucoSquareDimension,cameraMatrix,distCoeffs)
        if(markerID is not None):
            for i in range(len(markerID)):
                #aruco.draw
                aruco.drawAxis(frame,cameraMatrix,distCoeffs,r_vect[i],t_vect[i],0.1)
                aruco.drawDetectedMarkers(frame,markerCorners)
               # print("Corners",markerCorners[0][0])
                #focalLength = (abs(markerCorners[0][0][1][0]-markerCorners[0][0][2][0]) * KNOWN_DISTANCE) / KNOWN_WIDTH
                #print("Focal length: ",focalLength)
                print("Length: ",distance_to_camera(KNOWN_WIDTH,FOCAL,abs(markerCorners[0][0][1][0]-markerCorners[0][0][2][0])))
        cv2.imshow('Webcam',frame)
        if(cv2.waitKey(30)>=0): 
            break
    return 1
コード例 #6
0
    def __init__(self,
                 squaresX,
                 squaresY,
                 square_length,
                 marker_length,
                 marker_bits=4,
                 dict_size=50,
                 aruco_dict=None,
                 manually_verify=False):
        self.squaresX = squaresX
        self.squaresY = squaresY
        self.square_length = square_length
        self.marker_length = marker_length
        self.manually_verify = manually_verify

        dkey = (marker_bits, dict_size)
        self.dictionary = aruco.getPredefinedDictionary(ARUCO_DICTS[dkey])

        self.board = aruco.CharucoBoard_create(squaresX, squaresY,
                                               square_length, marker_length,
                                               self.dictionary)

        total_size = (squaresX - 1) * (squaresY - 1)

        objp = np.zeros((total_size, 3), np.float64)
        objp[:, :2] = np.mgrid[0:(squaresX - 1),
                               0:(squaresY - 1)].T.reshape(-1, 2)
        objp *= square_length
        self.objPoints = objp

        self.empty_detection = np.zeros((total_size, 1, 2)) * np.nan
        self.total_size = total_size
コード例 #7
0
def identifica_id(cv_image):
    gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)

    # --- Define the aruco dictionary
    aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_6X6_250)
    # parameters  = aruco.DetectorParameters_create()
    # parameters.minDistanceToBorder = 0
    # parameters.adaptiveThreshWinSizeMax = 1000

    corners, ids, rejectedImgPoints = aruco.detectMarkers(
        gray, aruco_dict)  # , parameters=parameters)

    aruco.drawDetectedMarkers(cv_image, corners, ids)

    if ids is None:
        ids = [[0]]
        centro = (0, 0)

    else:
        ids = ids.tolist()
        corners = corners[0][0]
        media_x = int(
            (corners[0][0] + corners[1][0] + corners[2][0] + corners[3][0]) /
            4)
        media_y = int(
            (corners[0][1] + corners[1][1] + corners[2][1] + corners[3][1]) /
            4)
        centro = (media_x, media_y)

    return centro, ids[0][0], ids[0]
コード例 #8
0
ファイル: common.py プロジェクト: dahburj/anipose
def get_calibration_board(config):
    board_size = config['calibration']['board_size']
    board_type = config['calibration']['board_type'].lower()

    if board_type in ['aruco', 'charuco']:
        dkey = (config['calibration']['board_marker_bits'],
                config['calibration']['board_marker_dict_number'])
        dictionary = aruco.getPredefinedDictionary(ARUCO_DICTS[dkey])
        if board_type == 'aruco':
            board = aruco.GridBoard_create(
                board_size[0], board_size[1],
                config['calibration']['board_marker_length'],
                config['calibration']['board_marker_separation_length'],
                dictionary)
        elif board_type == 'charuco':
            board = aruco.CharucoBoard_create(
                board_size[0], board_size[1],
                config['calibration']['board_square_side_length'],
                config['calibration']['board_marker_length'], dictionary)
    elif board_type == 'checkerboard':
        board = Checkerboard(board_size[0], board_size[1],
                             config['calibration']['board_square_side_length'])
    else:
        raise ValueError(
            "board_type should be one of "
            "'aruco', 'charuco', or 'checkerboard' not '{}'".format(
                board_type))

    return board
コード例 #9
0
    def __init__(self, marker_size, camera_matrix, camera_distortion):

        #The CvBridge command is only needed if the images are coming in via ROS
        #self.bridge = CvBridge()

        self.marker_size = marker_size
        self.camera_matrix = camera_matrix
        self.camera_distortion = camera_distortion

        self.R_flip = np.zeros((3, 3), dtype=np.float32)
        self.R_flip[0, 0] = 1.0
        self.R_flip[1, 1] = -1.0
        self.R_flip[2, 2] = -1.0

        #The following commands establish which Aruco dictionary will be in use.
        self.aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_5X5_100)
        self.parameters = aruco.DetectorParameters_create()

        #initialize video stream
        self.vid = cv2.VideoCapture(0)

        #initialize an empty frame within the class
        self.frame = np.zeros([960, 1280, 3], dtype=np.uint8)
        self.gray = np.zeros([960, 1280, 3], dtype=np.uint8)

        self.font = cv2.FONT_HERSHEY_PLAIN
コード例 #10
0
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
コード例 #11
0
def main():

    # establish global constants (hardcode in file or make optional param to fn call)
    # KNOWN_WIDTH is the width of the AR tags to detect in URC competition
    KNOWN_WIDTH = 20  # unit is cm

    # initialize the camera and grab a reference to the raw camera capture
    camera = PiCamera()
    camera.resolution = (640, 480)
    camera.framerate = 32
    rawCapture = PiRGBArray(camera, size=(640, 480))

    # allow the camera to warmup
    time.sleep(2)

    # capture frames from the camera
    for frame in camera.capture_continuous(rawCapture,
                                           format="bgr",
                                           use_video_port=True):
        # print("Captured Image")
        # grab the raw NumPy array representing the image
        image = np.array(frame.array)

        # Our operations on the frame come here
        gray_img = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
        #TODO: change "cv2.aruco.DICT_5x5" to rover aruco lib
        aruco_dict = aruco.getPredefinedDictionary(cv2.aruco.DICT_5X5_50)
        parameters = aruco.DetectorParameters_create()
        # print("Parameters:", parameters)

        # Lists of ids and the corners belonging to each id
        corners, ids, rejectedImgPoints = aruco.detectMarkers(
            gray_img, aruco_dict, parameters=parameters)
        # print("Corners:", corners)
        # print(type(id))

        if (len(corners) > 0):
            for i in range(len(corners)):  # used to be id
                # call function to calculate ar tag heading relative to rover
                depth = get_depth(i, corners[i])  #used to be id[i]
                heading = global_coord_trans(i, corners[i], depth)  # id[i]
                # this is where we would send heading over LCM channel
                # note: global coord trans could happen somewhere
                #       outside of this script. It doesn't, but it could
                print("Depth and Heading of", i, "are:", depth, "&", heading)
        gray_img = aruco.drawDetectedMarkers(gray_img, corners)
        #print(rejectedImgPoints)

        # Display the resulting frame
        cv2.imshow('frame', gray_img)
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break
        rawCapture.truncate(0)

    # When everything done, release the capture
    # cap.release()
    cv2.destroyAllWindows()

    return
コード例 #12
0
ファイル: findRt.py プロジェクト: Chen-Yu-Qiang/uav_sim
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)
コード例 #13
0
ファイル: ar.py プロジェクト: masa-15/practice
 def __init__(self, videoPort, cameraMatrix, distortionCoefficients):
     #カメラ読み込み
     self.cap = cv2.VideoCapture(videoPort)
     #カメラの内部パラメータ読み込み
     self.cameraMatrix = np.load(cameraMatrix)
     self.distortionCoefficients = np.load(distortionCoefficients)
     #arucoの辞書読み込み
     self.dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
コード例 #14
0
	def __init__(self):
		self._cap = cv2.VideoCapture(VIDEO_SOURCE_ID)
		self._dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50) 
		self._font = cv2.FONT_HERSHEY_SIMPLEX
		rospy.Subscriber("/neato01/pose", PoseStamped, self.tracked_neato01, queue_size=10)
		cv2.namedWindow('frame', cv2.WINDOW_NORMAL)
		cv2.resizeWindow('frame', 1280, 720)
		self._start_time = time.time()
コード例 #15
0
ファイル: piScan.py プロジェクト: mujahidatar/Antbot
def detect_Aruco(image):               
    img=image
    aruco_list = {}
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_7X7_250)
    parameters = aruco.DetectorParameters_create()  
    _, ids, _ = aruco.detectMarkers(gray, aruco_dict, parameters = parameters)
    return ids
コード例 #16
0
def configure_rigid_bodies(configuration):
    """
    reads configuration and creates a list of rigid bodies
    together with a list of dictionaries used.
    """
    dictionary_name = configuration.get("aruco dictionary", 'DICT_4X4_50')
    try:
        ar_dictionary_name = getattr(aruco, dictionary_name)
    except AttributeError:
        raise ImportError(('Failed when trying to import {} from cv2.'
                           'aruco. Check dictionary exists.'
                           ).format(dictionary_name)) from AttributeError
    ar_dicts = []
    ar_dict_names = []
    ar_dicts.append(aruco.getPredefinedDictionary(ar_dictionary_name))
    ar_dict_names.append(dictionary_name)

    rigid_bodies = []
    rigid_body_configs = configuration.get('rigid bodies', [])

    for rigid_body_config in rigid_body_configs:
        rigid_body = ArUcoRigidBody(rigid_body_config.get('name', 'no name'))
        filename = rigid_body_config.get('filename', None)
        if filename is None:
            raise ValueError('rigid body configuration must include filename')
        dictionary_name = rigid_body_config.get('aruco dictionary',
                                                'DICT_ARUCO_ORIGINAL')
        try:
            ar_dictionary_name = getattr(aruco, dictionary_name)
        except AttributeError:
            raise ImportError(('Failed when trying to import {} from cv2.'
                               'aruco. Check dictionary exists.'
                               ).format(dictionary_name)) from AttributeError

        rigid_body.load_3d_points(filename, dictionary_name)

        tag_width = rigid_body_config.get('tag width', None)
        if tag_width is not None:
            rigid_body.scale_3d_tags(tag_width)

        rigid_bodies.append(rigid_body)
        if dictionary_name not in ar_dict_names:
            ar_dict_names.append(dictionary_name)
            ar_dicts.append(aruco.getPredefinedDictionary(ar_dictionary_name))

    return ar_dicts, ar_dict_names, rigid_bodies
コード例 #17
0
    def show_camera(self):

        flag, self.image = self.cap.read()
        if self.aruco_flag is True:

            if self.camera_matrix is None:
                error_dialog = QErrorMessage()
                error_dialog.showMessage(
                    'Oh no you do not have load the matrix\n'
                    'Please sure you have clicked the load mtx button\n'
                    'Program will close in 1 sec\n')
                error_dialog.exec_()
                exit()

            # load the matrix in Camera Matrix
            aruco_dict = aruco.getPredefinedDictionary(
                aruco.DICT_ARUCO_ORIGINAL)
            parameters = aruco.DetectorParameters_create()

            gray = cv2.cvtColor(self.image, cv2.COLOR_BGR2GRAY)
            ret, out1 = cv2.threshold(gray, 90, 255, cv2.THRESH_BINARY)
            corners, ids, rejected = aruco.detectMarkers(
                image=out1,
                dictionary=aruco_dict,
                parameters=parameters,
                cameraMatrix=self.camera_matrix,
                distCoeff=self.camera_distortion)
            if ids is not None:
                # -- Find the marker's 6-DOF pose
                rvecs, tvecs, _objPoints = aruco.estimatePoseSingleMarkers(
                    corners, 140, self.camera_matrix, self.camera_distortion)
                # print(rvecs)
                for i in range(rvecs.shape[0]):
                    aruco.drawAxis(self.image, self.camera_matrix,
                                   self.camera_distortion, rvecs[i, :, :],
                                   tvecs[i, :, :], 100)
                    aruco.drawDetectedMarkers(self.image, corners)
                id_str = "id:{}".format(ids[0][0])
                rvecs_str = "rvecs:Rx: {:.2f},Ry: {:.2f},Rz: {:.2f}".format(
                    rvecs[0][0][0], rvecs[0][0][1], rvecs[0][0][2])
                tvecs_str = "tvecs:Tx: {:.3f},Ty: {:.3f},Tz: {:.3f}".format(
                    tvecs[0][0][0], tvecs[0][0][1], tvecs[0][0][2])
                center_point_str = f"center point {corners}"
                # cv2.putText(self.image, id_str, (0, 30), cv2.FONT_HERSHEY_SIMPLEX, 3, (0, 255, 0), 5)
                # cv2.putText(self.image, rvecs_str, (0, 130), cv2.FONT_HERSHEY_SIMPLEX, 3, (0, 255, 0), 5)
                # cv2.putText(self.image, tvecs_str, (0, 230), cv2.FONT_HERSHEY_SIMPLEX, 3, (0, 255, 0), 5)
                # cv2.putText(self.image, center_point_str, (0, 330), cv2.FONT_HERSHEY_SIMPLEX, 2, (0, 255, 0), 5)
                self.tvec_label.setText(tvecs_str)
                self.rvec_label.setText(rvecs_str)
        else:
            pass
        show = cv2.resize(self.image, (1920, 1080))
        show = cv2.cvtColor(show, cv2.COLOR_BGR2RGB)
        showImage = QImage(show.data, show.shape[1], show.shape[0],
                           QImage.Format_RGB888)
        self.label_show_camera.setPixmap(QPixmap.fromImage(showImage))
        self.label_show_camera.resize(self.widget.size())
        self.label_show_camera.setScaledContents(True)
コード例 #18
0
def createArucoMarkers():
    outputMarker = []
    markerDictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
    for i in range(50):

        outputMarker = aruco.drawMarker(markerDictionary, i, 500)
        imageName = "4x4Marker_" + str(i) + ".jpg"
        #f=open(imageName,"w")
        cv2.imwrite(imageName, outputMarker)
コード例 #19
0
 def test_get_dictionary(self):
     """
     compare public attributes of dictionary to confirm equality
     """
     err_msg = "get_dictionary failed"
     dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
     self.assertTrue(
         FiducialMarker.dictionary_equal(dictionary,
                                         FiducialMarker.get_dictionary()))
コード例 #20
0
def detecter():
    cap = cv2.VideoCapture(2)
    Setcamera(cap)

    cameraMartix = np.array([[720.2120, 2.0761, 335.1827],
                             [0, 721.1528, 268.9376], [0, 0, 1]])

    distCoeffs = np.array([-0.1622, 0.1400, 0.00234976, -0.0008216553])

    dict = aruco.getPredefinedDictionary(aruco.DICT_6X6_250)
    pub = rospy.Publisher('visual', Visual_msg, queue_size=10)
    rospy.init_node('detecter', anonymous=True)
    rate = rospy.Rate(100)  # 10hz
    while not rospy.is_shutdown():
        ret, frame = cap.read()
        img = frame
        corners, ids, reject = aruco.detectMarkers(frame, dict)
        if ids is not None:
            img = aruco.drawDetectedMarkers(img, corners, ids)
            r, t = aruco.estimatePoseSingleMarkers(corners, 0.95, cameraMartix,
                                                   distCoeffs)
            position = xyz(r, t)
            # rospy.loginfo("r:\n",r)
            # rospy.loginfo("t:\n",t)
            for i, id in enumerate(ids):
                img = aruco.drawAxis(img, cameraMartix, distCoeffs, r[i], t[i],
                                     1)
                msg = Visual_msg()
                msg.id = id
                marker_r = r[i][0]
                marker_t = t[i][0]
                msg.rvec.x = marker_r[0]
                msg.rvec.y = marker_r[1]
                msg.rvec.z = marker_r[2]
                msg.tvec.x = position[0]
                msg.tvec.y = position[1]
                msg.tvec.z = position[2]
                rospy.loginfo("detect marker")
                pub.publish(msg)
                rate.sleep()
            cv2.imshow("img", img)
            cv2.waitKey(1)
        else:
            cv2.imshow("img", frame)
            cv2.waitKey(1)
            msg = Visual_msg()
            msg.id = 100
            msg.rvec.x = 0
            msg.rvec.y = 0
            msg.rvec.z = 0
            msg.tvec.x = 0
            msg.tvec.y = 0
            msg.tvec.z = 0
            rospy.loginfo("detect nothing")
            rospy.loginfo(msg)
            pub.publish(msg)
コード例 #21
0
def main_boi():
    while (True):
        ret, frame = cap.read()

        #gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        aruco_dict = aruco.getPredefinedDictionary(0)
        parameters = aruco.DetectorParameters_create()

        # draw boundary
        cv2.rectangle(frame, (50, 50), (1230, 670), (255, 0, 0), 2)

        corners, ids, rejectedImgPoints = aruco.detectMarkers(
            frame, aruco_dict, parameters=parameters)
        for box_index in range(len(corners)):

            x_avg = get_avg_coord(corners[box_index][0])[0]
            y_avg = get_avg_coord(corners[box_index][0])[1]

            cv2.rectangle(frame, (int(x_avg), int(y_avg)),
                          (int(x_avg + 5), int(y_avg + 5)), (0, 255, 0), 2)
            # if start
            if ids[box_index][0] == start_id:
                start = corners[box_index][0]
                print "start: " + str(get_avg_coord(start))

            # if goal
            elif ids[box_index][0] == goal_id:
                goal = corners[box_index][0]
                print "goal: " + str(get_avg_coord(goal))

            # if obs
            for i in range(len(obs_id)):
                if ids[box_index][0] in obs_id:
                    obs[i] = get_avg_coord(corners[box_index][0])
                    print "obs: " + str(obs)

            # if robot
            for i in range(len(robot_id)):
                if ids[box_index][0] in robot_id:
                    robot[i] = get_avg_coord(corners[box_index][0])
                    print "robot: " + str(robot)

            cv2.putText(
                frame, "id=" + str(ids[box_index][0]) + " (" +
                str(int(x_avg)) + "," + str(int(y_avg)) + ")",
                (int(x_avg), int(y_avg)), cv2.FONT_HERSHEY_SIMPLEX, 0.5,
                (0, 0, 255))

        gray = aruco.drawDetectedMarkers(frame, corners)

        draw_grid(frame, num_rows, num_cols, corners, ids)

        # Display the resulting frame
        cv2.imshow('frame', frame)
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break
コード例 #22
0
    def detect_aruco(self):
        self.aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_4X4_250)

        image = self.image_repository.get_next_image()
        gray = cv2.cvtColor(image.frame, cv2.COLOR_BGR2GRAY)
        corners, ids, rejectedImgPoints = aruco.detectMarkers(
            gray,
            self.aruco_dict,
            parameters=aruco.DetectorParameters_create())
        return corners
コード例 #23
0
ファイル: detect.py プロジェクト: mc0239/fireflies
def init(debug=False):
    global sock
    sock = SocketClient(timeout=1)

    global capture
    capture = CaptureThread(debug)
    capture.start()

    global markerDict
    markerDict = aruco.getPredefinedDictionary(aruco.DICT_4X4_250)
コード例 #24
0
ファイル: 1.5.py プロジェクト: pssrivatava/project
def aruco_detect1():
    
    aruco_list = {}
    rgb = image1[...,::-1]
    gray = cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB)
    
    
    aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_5X5_1000)#aruco.Dictionary_get(aruco.DICT_4X4_50)   #creating aruco_dict with 5x5 bits with max 250 ids..so ids ranges from 0-249
    print(aruco_dict)
    parameters = aruco.DetectorParameters_create()  #refer opencv page for clarification
        #lists of ids and the corners beloning to each id
    print(parameters)
    corners, ids, _ = aruco.detectMarkers(gray, aruco_dict, parameters = parameters)
        #corners is the list of corners(numpy array) of the detected markers. For each marker, its four corners are returned in their original order (which is clockwise starting with top left). So, the first corner is the top left corner, followed by the top right, bottom right and bottom left.
    #print len(corners), corners, ids
    print(corners)
    print(len(corners))
    gray = aruco.drawDetectedMarkers(gray, corners,ids)
    cv2.imshow('frame',gray)
    print (type(corners[0]))
    if len(corners):    #returns no of arucos
        print (len(corners))
        print (len(ids))
        for k in range(len(corners)):
            temp_1 = corners[k]
            temp_1 = temp_1[0]
            temp_2 = ids[k]
            temp_2 = temp_2[0]
            aruco_list[temp_2] = temp_1
        return aruco_list
  
        
   


    
   
    
    img = cv2.imread(path_to_image)     #give the name of the image with the complete path
    id_aruco_trace = 0
    det_aruco_list = {}
    img2 = img[0:x,0:y,:]   #separate out the Aruco image from the whole image
    det_aruco_list = detect_Aruco(img2)
    if det_aruco_list:
        img3 = mark_Aruco(img2,det_aruco_list)
        id_aruco_trace = calculate_Robot_State(img3,det_aruco_list)
        print(id_aruco_trace)        
        cv2.imshow('image',img2)
        cv2.waitKey(0)
        
     
        
        
        
    cv2.destroyAllWindows()
コード例 #25
0
    def show_aruco(self):
        aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_4X4_250)

        image = aruco.drawMarker(aruco_dict, 3, 700)

        photo = ImageTk.PhotoImage(image=PIL.Image.fromarray(image))

        self.projector_window.canvas.create_image(500, 400, image=photo)
        return np.array(
            [[150, 50, 0], [850, 50, 0], [850, 750, 0], [150, 750, 0]],
            dtype=np.float32)
コード例 #26
0
ファイル: gen_aruco.py プロジェクト: tbh111/DroneSim
def gen_aruco(n):
    while(n):

        img = np.zeros((100,100,3), np.uint8)
        Dictionary = aruco.getPredefinedDictionary(aruco.DICT_ARUCO_ORIGINAL)
        img=aruco.drawMarker(Dictionary,n,100,img,1)
        #cv2.imwrite('C:\\Users\\ailab\\PycharmProjects\\drone\\train\\'+str(n)+'.jpg',img)
        cv2.imwrite('C:\\Users\\ailab\\PycharmProjects\\drone\\train\\class_0\\' + 'aruco_0_'+str(n) + '.jpg', img)
        print('gen_aruco:  '+'aruco_0_'+str(n) + '.jpg')
        n = n - 1
    return 0
コード例 #27
0
def get_markers(frame):
    '''Função que pega os marcadores da frame passada como argumento e retorna três listas
    corners (com as coordenadas de cada ID detectado), ids (com o ID detectado) e rejectedImgPoints
    (com as coordenadas rejeitada)'''
    aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_6X6_250)
    parameters = aruco.DetectorParameters_create()

    corners, ids, rejectedImgPoints = aruco.detectMarkers(
        frame, aruco_dict, parameters=parameters)

    return corners, ids, rejectedImgPoints
コード例 #28
0
def example_calibrate_aruco_markers(filename_in, marker_length_mm = 3.75, marker_space_mm = 0.5, dct = aruco.DICT_6X6_1000):

    image = cv2.imread(filename_in)
    gray = tools_image.desaturate(image)

    scale = (marker_length_mm / 2, marker_length_mm / 2, marker_length_mm / 2)
    num_cols, num_rows = 4,5
    board = aruco.GridBoard_create(num_cols, num_rows, marker_length_mm, marker_space_mm,aruco.getPredefinedDictionary(dct))
    image_AR, image_cube = gray.copy(), gray.copy()
    base_name, ext = filename_in.split('/')[-1].split('.')[0], filename_in.split('/')[-1].split('.')[1]
    #board_width_px = int((num_cols * marker_length_mm + (num_cols - 1) * marker_space_mm))
    #board_height_px= int((num_rows * marker_length_mm + (num_rows - 1) * marker_space_mm))
    #image_board = aruco.drawPlanarBoard(board, (board_width_px, board_height_px))


    camera_matrix = None#tools_pr_geom.compose_projection_mat_3x3(image.shape[1], image.shape[0])
    corners, ids, _ = aruco.detectMarkers(cv2.cvtColor(image, cv2.COLOR_BGR2GRAY), aruco.getPredefinedDictionary(dct))

    if len(corners)>0:
        if len(corners)==1:corners,ids = numpy.array([corners]),numpy.array([ids])
        else:corners, ids = numpy.array(corners), numpy.array(ids)
        counters = numpy.array([len(ids)])

        ret, camera_matrix, dist, rvecs, tvecs = aruco.calibrateCameraAruco(corners,ids,counters, board, gray.shape[:2],None, None)
        image_markers = [tools_image.saturate(aruco.drawMarker(aruco.getPredefinedDictionary(dct), id, 100)) for id in ids]

        #!!!
        #camera_matrix = tools_pr_geom.compose_projection_mat_3x3(4200,4200)
        for i,image_marker in enumerate(image_markers):
            rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(corners[i], marker_length_mm, camera_matrix, numpy.zeros(5))


            image_AR  = tools_render_CV.draw_image(image_AR,image_marker, camera_matrix, numpy.zeros(5), numpy.array(rvecs).flatten(), numpy.array(tvecs).flatten(),scale)
            image_cube = tools_render_CV.draw_cube_numpy(image_cube, camera_matrix, numpy.zeros(5), numpy.array(rvecs).flatten(),numpy.array(tvecs).flatten(), scale)

    cv2.imwrite(folder_out + base_name+'_AR.png', image_AR)
    cv2.imwrite(folder_out + base_name+'_AR_cube.png', image_cube)

    print(camera_matrix)

    return camera_matrix
コード例 #29
0
def create_markers():
    j = 2
    markers_dict_1 = aruco.getPredefinedDictionary(aruco.DICT_6X6_50)
    markers_dict1 = custom_dictionary_from(j, 6, markers_dict_1)
    markers_dict_2 = aruco.getPredefinedDictionary(aruco.DICT_7X7_50)
    markers_dict2 = custom_dictionary_from(j, 7, markers_dict_2)

    markers1 = list(
        aruco.drawMarker(markers_dict1, i + 1, 200) for i in range(1))
    markers2 = list(
        aruco.drawMarker(markers_dict2, i + 1, 200) for i in range(1))

    for i in range(len(markers1)):
        cv2.imwrite(
            os.path.join('markers', 'marker6_') + str(i + 1) + '.png',
            markers1[i])
    for i in range(len(markers2)):
        cv2.imwrite(
            os.path.join('markers', 'marker7_') + str(i + 1) + '.png',
            markers2[i])
    return markers_dict1, markers_dict2
コード例 #30
0
ファイル: capture_thread.py プロジェクト: mc0239/fireflies
 def __threadfunc__(self):
     markerDict = aruco.getPredefinedDictionary(aruco.DICT_4X4_250)
     while self.working:
         ok, frame = self.capture.read()
         self.current_frame = frame
         
         if self.debug:
             (corners, ids, rejects) = aruco.detectMarkers(frame, markerDict)
             frame = aruco.drawDetectedMarkers(frame, corners, ids)
             cv2.imshow("Camera", cv2.flip(imutils.resize(frame, width=480), 1))
            
         cv2.waitKey(1)