Python cv2.aruco方法代碼示例

# 需要導入模塊: import cv2 [as 別名]
# 或者: from cv2 import aruco [as 別名]
def get_aruco_center(cad,d):
     gray = cv2.cvtColor(cad, cv2.COLOR_BGR2GRAY)
     aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
     parameters = aruco.DetectorParameters_create()
     #lists of ids and the corners beloning to each id
     corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters)
     XYZ = []
     if np.all(ids != None):
          for index,cornerset in enumerate(corners):
               cornerset = cornerset[0]
               for corner in cornerset:
                    if d[int(corner[1])][int(corner[0])][2]!= 0:
                         XYZ.append(d[int(corner[1])][int(corner[0])])

     XYZ = np.asarray(XYZ)
     return np.mean(XYZ, axis = 0) 

# 需要導入模塊: import cv2 [as 別名]
# 或者: from cv2 import aruco [as 別名]
def findplane(cad,d):
    p0 = [0.506645455682, -0.185724560275, -1.43998120646, 1.37626378129]
    sol = None
    gray = cv2.cvtColor(cad, cv2.COLOR_BGR2GRAY)
    aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
    parameters = aruco.DetectorParameters_create()
    #lists of ids and the corners beloning to each id
    corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters)
    XYZ = [[],[],[]]
    if np.all(ids != None):
        for index,cornerset in enumerate(corners):
            cornerset = cornerset[0]
            for corner in cornerset:
                if d[int(corner[1])][int(corner[0])][2]!= 0:
                    XYZ[0].append(d[int(corner[1])][int(corner[0])][0])
                    XYZ[1].append(d[int(corner[1])][int(corner[0])][1])
                    XYZ[2].append(d[int(corner[1])][int(corner[0])][2])


        XYZ = np.asarray(XYZ)
        sol = leastsq(residuals, p0, args=(None, XYZ))[0]

    return sol 

# 需要導入模塊: import cv2 [as 別名]
# 或者: from cv2 import aruco [as 別名]
def print_usage():
    
    print("Usage: aruco.py <path>")
    print("path: all or name of the folder")
    print("e.g., aruco.py all, aruco.py.py LINEMOD/Cheezit") 

# 需要導入模塊: import cv2 [as 別名]
# 或者: from cv2 import aruco [as 別名]
def marker_registration(source,target):
     cad_src, depth_src = source
     cad_des, depth_des = target
 
     gray_src = cv2.cvtColor(cad_src, cv2.COLOR_RGB2GRAY)
     gray_des = cv2.cvtColor(cad_des, cv2.COLOR_RGB2GRAY)
     aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
     parameters = aruco.DetectorParameters_create()
    
     #lists of ids and the corners beloning to each id
     corners_src, _ids_src, rejectedImgPoints = aruco.detectMarkers(gray_src, aruco_dict, parameters=parameters)
     corners_des, _ids_des, rejectedImgPoints = aruco.detectMarkers(gray_des, aruco_dict, parameters=parameters)
     try:
         ids_src = []
         ids_des = []
         for i in range(len(_ids_src)):
              ids_src.append(_ids_src[i][0])
         for i in range(len(_ids_des)):
              ids_des.append(_ids_des[i][0])
     except:
         return None

     common = [x for x in ids_src if x in ids_des]
  
     if len(common) < 2:
          # too few marker matches, use icp instead
          return None

     
     src_good = []
     dst_good = []
     for i,id in enumerate(ids_des):
          if id in ids_src:
               j = ids_src.index(id)
               for count,corner in enumerate(corners_src[j][0]):
                    feature_3D_src = depth_src[int(corner[1])][int(corner[0])]
                    feature_3D_des = depth_des[int(corners_des[i][0][count][1])][int(corners_des[i][0][count][0])]
                    if feature_3D_src[2]!=0 and feature_3D_des[2]!=0:
                         src_good.append(feature_3D_src)
                         dst_good.append(feature_3D_des)
    
     # get rigid transforms between 2 set of feature points through ransac
     try:
          transform = match_ransac(np.asarray(src_good),np.asarray(dst_good))
          return transform
     except:
          return None 

# 需要導入模塊: import cv2 [as 別名]
# 或者: from cv2 import aruco [as 別名]
def __init__(self,
                id_to_find,
                marker_size,
                camera_matrix,
                camera_distortion,
                camera_size=[640,480],
                show_video=False
                ):
        
        
        self.id_to_find     = id_to_find
        self.marker_size    = marker_size
        self._show_video    = show_video
        
        self._camera_matrix = camera_matrix
        self._camera_distortion = camera_distortion
        
        self.is_detected    = False
        self._kill          = False
        
        #--- 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 videocamera (this may also be a video or a picture)
        self._cap = cv2.VideoCapture(0)
        #-- Set the camera size as the one it was calibrated with
        self._cap.set(cv2.CAP_PROP_FRAME_WIDTH, camera_size[0])
        self._cap.set(cv2.CAP_PROP_FRAME_HEIGHT, camera_size[1])

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

        self._t_read      = time.time()
        self._t_detect    = self._t_read
        self.fps_read    = 0.0
        self.fps_detect  = 0.0