def SeekDuelist(): for times in range(4): info("SeekDuelist() getting screenshot") #get screen image Screenshot() #iterate through sprites info("SeekDuelist() iterating through duelists") for image in listdir("TEMPLATES\\characters\\"): #set sprite pic="TEMPLATES\\characters\\"+image #read sprite image img1= imread(pic,IMREAD_GRAYSCALE) #read screen image img2= imread("screen.png",IMREAD_GRAYSCALE) #set ORB object orb= ORB_create(100000) #feature detect sprite kp1, des1= orb.detectAndCompute(img1, None) #feature detect screen kp2, des2= orb.detectAndCompute(img2, None) #match features from sprite and screen bf = BFMatcher(NORM_HAMMING, crossCheck=True) matches = bf.match(des1, des2) #sort accuracy of matches matches = sorted(matches,key=lambda image:image.distance) #select accurate matches found=[] for m in matches: if m.distance<=20: found.append(m.distance) #check if accurate matches were found if len(found)!=0: #set image info img2_idx = matches[0].trainIdx # Get the coordinates # x - left to right # y - top to bottom (x,y) = kp2[img2_idx].pt info("SeekDuelist() match found for "+image+" at ("+str(int(x))+", "+str(int(y))+")") return (int(x),int(y)) #Scroll one down to change screens info("SeekDuelist() no matches found. scrolling") mouse_event(MOUSEEVENTF_WHEEL, 0, 0, -1, 0) sleep(2) info("SeekDuelist() no matches found. raising exception") raise Exception
class OrbFeaturesHandler(FeaturesHandlerAbstractBase): """ This class implements an ORB features exractor """ features_extractor: ORB features_matcher: BFMatcher DEFAULT_DISTANCE_THRESHOLD_FOR_SUCCESSFULL_FEATURE_MATCH: int = 10 DEFAULT_NUMBER_OF_FEATURES = 1000 number_of_features: int def __init__(self, number_of_features: int = None): if number_of_features is None: self.number_of_features = self.DEFAULT_NUMBER_OF_FEATURES self.features_extractor = ORB_create( nfeatures=self.DEFAULT_NUMBER_OF_FEATURES) # ORB uses binary descriptors -> use hamming norm (XOR between descriptors) self.features_matcher = BFMatcher(NORM_HAMMING, crossCheck=True) def extract_features(self, frame: np.ndarray) -> ProcessedFrameData: """ This method extracts ORB features """ list_of_keypoints, descriptors = self.features_extractor.detectAndCompute( image=frame, mask=None) return ProcessedFrameData.build(frame=frame, list_of_keypoints=list_of_keypoints, descriptors=descriptors) def match_features(self, frame_1: ProcessedFrameData, frame_2: ProcessedFrameData): """ This method matches ORB features based on https://docs.opencv.org/master/dc/dc3/tutorial_py_matcher.html """ list_of_matches: List[DMatch] = self.features_matcher.match( queryDescriptors=frame_1.descriptors, trainDescriptors=frame_2.descriptors) return sorted(list_of_matches, key=lambda x: x.distance) # # Sort them in the order of their distance. # return def is_handler_capable(self, frame: np.ndarray) -> bool: """ This method implements ORB handler capability test """ extracted_features: ProcessedFrameData = self.extract_features( frame=frame) return len(extracted_features.list_of_keypoints) >= int( 0.9 * self.DEFAULT_NUMBER_OF_FEATURES)
class OrbFeature2D: def __init__(self, num_features=2000, scale_factor=1.2, num_levels=8): print('Using Orb Feature 2D') self.orb_extractor = ORB_create(num_features, scale_factor, num_levels) # extract keypoints def detect(self, img): # detect kps_tuples = self.orb_extractor.detect(img) # convert keypoints kps = [cv2.KeyPoint(*kp) for kp in kps_tuples] return kps def detectAndCompute(self, img): #detect and compute kps, des = self.orb_extractor.detectAndCompute(img, None) return kps, des
def algorithm_ORB(self, photo, screen, screen_colored, nfeatures=4500, descriptor_matcher_name='BruteForce-Hamming'): t1 = time.perf_counter() # Init algorithm orb = ORB_create(nfeatures) t2 = time.perf_counter() self.writeLog('Created ORB object - {}ms'.format( self.formatTimeDiff(t1, t2))) # Detect and compute kp_photo, des_photo = orb.detectAndCompute(photo, None) kp_screen, des_screen = orb.detectAndCompute(screen, None) t3 = time.perf_counter() self.writeLog('Detected keypoints - {}ms'.format( self.formatTimeDiff(t2, t3))) # Descriptor Matcher try: descriptor_matcher = DescriptorMatcher_create( descriptor_matcher_name) except: return False t4 = time.perf_counter() self.writeLog('Initialized Descriptor Matcher - {}ms'.format( self.formatTimeDiff(t3, t4))) # Calc knn Matches try: matches = descriptor_matcher.knnMatch(des_photo, des_screen, k=2) except: return False t5 = time.perf_counter() self.writeLog('Calced knn matches - {}ms'.format( self.formatTimeDiff(t4, t5))) if not matches or len(matches) == 0: return False # store all the good matches as per Lowe's ratio test. good = [] for m, n in matches: if m.distance < 0.75 * n.distance: good.append(m) t6 = time.perf_counter() self.writeLog('Filtered good matches - {}ms'.format( self.formatTimeDiff(t5, t6))) if not good or len(good) < 20: return False photo_pts = np.float32([kp_photo[m.queryIdx].pt for m in good]).reshape(-1, 1, 2) # pylint: disable=too-many-function-args screen_pts = np.float32([kp_screen[m.trainIdx].pt for m in good]).reshape(-1, 1, 2) # pylint: disable=too-many-function-args M, _ = findHomography(photo_pts, screen_pts, RANSAC, 5.0) t7 = time.perf_counter() self.writeLog('Found Homography - {}ms'.format( self.formatTimeDiff(t6, t7))) if M is None or not M.any() or len(M) == 0: return False h, w = photo.shape pts = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1], [w - 1, 0]]).reshape(-1, 1, 2) # pylint: disable=too-many-function-args dst = perspectiveTransform(pts, M) t8 = time.perf_counter() self.writeLog('Perspective Transform - {}ms'.format( self.formatTimeDiff(t7, t8))) minX = dst[0][0][0] minY = dst[0][0][1] maxX = dst[0][0][0] maxY = dst[0][0][1] for i in range(4): if dst[i][0][0] < minX: minX = dst[i][0][0] if dst[i][0][0] > maxX: maxX = dst[i][0][0] if dst[i][0][1] < minY: minY = dst[i][0][1] if dst[i][0][1] > maxY: maxY = dst[i][0][1] minX = int(minX) minY = int(minY) maxX = int(maxX) maxY = int(maxY) if minX < 0: minX = 0 if minY < 0: minY = 0 logging.info('minY {}'.format(int(minY))) logging.info('minX {}'.format(int(minX))) logging.info('maxY {}'.format(int(maxY))) logging.info('maxX {}'.format(int(maxX))) if maxX - minX <= 0: return False if maxY - minY <= 0: return False imwrite(self.match_dir + '/result.png', screen_colored[int(minY):int(maxY), int(minX):int(maxX)]) t9 = time.perf_counter() self.writeLog('Wrote Image - {}ms'.format(self.formatTimeDiff(t8, t9))) return True
class ROS_Video_Node(): def __init__(self, name): """This initializes the object""" self.name = name self.last_frame = None self.current_frame = None self.match_frames = None #Create an ORB object for keypoint detection self.orb = ORB_create(nfeatures=100, scaleFactor=2, edgeThreshold=100, fastThreshold=10) #Create a BF object for keypoint matching self.bf = BFMatcher(NORM_L1, crossCheck=True) #For keeping track of the drone's current velocity self.vel_x = None self.vel_y = None #For plotting the drone's current and past velocities self.times = [] self.x_s = [] self.y_s = [] #Creating a publisher that will publish the calculated velocity to the ROS topic /quadrotor/ardrone/calc_vel self.pub = rospy.Publisher("/quadrotor/ardrone/calc_vel", Twist, queue_size=10) def detect_motion(self): """This function detects the motion between the current and last frame.""" if (self.last_frame == self.current_frame).all(): print( "Beep boop! I think my current frame is exactly the same as my last frame. \nEither I'm not moving, or something is very wrong.\n" ) namedWindow(self.name) imshow(self.name, self.current_frame) waitKey(1) return None #This finds the keypoints and descriptors with SIFT kp1, des1 = self.orb.detectAndCompute(self.last_frame, None) kp2, des2 = self.orb.detectAndCompute(self.current_frame, None) #Match descriptors matches = self.bf.match(des1, des2) #Sort them in the order of their distance matches = sorted(matches, key=lambda x: x.distance) #This is a test to see if I can filter out bad matches from the getgo #Right now I've set a threshold that I think will keep only bad matches to see if it makes the data super noisy # print("MATCH DISTANCES") #undoiing test, ELEPHANT filtered_matches = matches # filtered_matches = [] # for match in matches: # if match.distance > 1000: # filtered_matches.append(match) if len(filtered_matches) < 5: print( "Beep boop! Not enough good matches were found. This data is unreliable. \n Try moving me above the building, please.\n" ) namedWindow(self.name) imshow(self.name, self.current_frame) waitKey(1) return None #create arrays of the coordinates for keypoints in the two frames kp1_coords = asarray( [kp1[mat.queryIdx].pt for mat in filtered_matches]) kp2_coords = asarray( [kp2[mat.trainIdx].pt for mat in filtered_matches]) #calculate the translations needed to get from the first set of keypoints to the next set of keypoints translations = kp2_coords - kp1_coords least_error = 5 for translation in translations: a = translation[0] * ones((translations.shape[0], 1)) b = translation[1] * ones((translations.shape[0], 1)) test_translation = concatenate((a, b), axis=1) test_coords = kp1_coords + test_translation error = sqrt(mean(square(kp2_coords - test_coords))) if error < least_error: least_error = error best_translation = translation self.vel_x = translation[0] self.vel_y = translation[1] # Draw first matches. draw_params = dict( matchColor=(0, 255, 0), # draw matches in green color singlePointColor=None, matchesMask=None, # draw only inliers flags=2) self.match_frames = drawMatches(self.last_frame, kp1, self.current_frame, kp2, matches, None, **draw_params) namedWindow(self.name) imshow(self.name, self.match_frames) waitKey(1) if least_error < 2: print("This is a new match") print("X velocity: ", self.vel_x) print("Y velocity: ", self.vel_y) print("least_error: ", least_error) #Publish the velocities found calc_vel = Twist() calc_vel.linear.x = self.vel_x calc_vel.linear.y = self.vel_y self.pub.publish(calc_vel) def plot_current_vel(self): """This function live plots the quadrotor's measured velocity""" #save the current velocities self.times.append(time()) self.x_s.append(self.vel_x) self.y_s.append(self.vel_y) #plot the velocities plt.title("Live Velocities") plt.ylabel("Velocity") plt.xlabel("Time") plt.ylim(-20, 20) try: plt.plot(self.times[-60:], self.x_s[-60:], 'r-', label="X Velocity") plt.plot(self.times[-60:], self.y_s[-60:], 'b-', label="Y Velocity") except: plt.plot(self.times, self.x_s, 'r-', label="X Velocity") plt.plot(self.times, self.y_s, 'b-', label="Y Velocity") plt.legend(loc="upper right")
class ServoingDesignedFeaturesSimpleQuadPanda3dEnv(SimpleQuadPanda3dEnv, ServoingEnv): def __init__(self, action_space, feature_type=None, filter_features=None, max_time_steps=100, distance_threshold=4.0, **kwargs): """ filter_features indicates whether to filter out key points that are not on the object in the current image. Key points in the target image are always filtered out. """ SimpleQuadPanda3dEnv.__init__(self, action_space, **kwargs) ServoingEnv.__init__(self, env=self, max_time_steps=max_time_steps, distance_threshold=distance_threshold) lens = self.camera_node.node().getLens() self._observation_space.spaces['points'] = BoxSpace( np.array([-np.inf, lens.getNear(), -np.inf]), np.array([np.inf, lens.getFar(), np.inf])) film_size = tuple(int(s) for s in lens.getFilmSize()) self.mask_camera_sensor = Panda3dMaskCameraSensor( self.app, (self.skybox_node, self.city_node), size=film_size, near_far=(lens.getNear(), lens.getFar()), hfov=lens.getFov()) for cam in self.mask_camera_sensor.cam: cam.reparentTo(self.camera_sensor.cam) self.filter_features = True if filter_features is None else False self._feature_type = feature_type or 'sift' if cv2.__version__.split('.')[0] == '3': from cv2.xfeatures2d import SIFT_create, SURF_create from cv2 import ORB_create if self.feature_type == 'orb': # https://github.com/opencv/opencv/issues/6081 cv2.ocl.setUseOpenCL(False) else: SIFT_create = cv2.SIFT SURF_create = cv2.SURF ORB_create = cv2.ORB if self.feature_type == 'sift': self._feature_extractor = SIFT_create() elif self.feature_type == 'surf': self._feature_extractor = SURF_create() elif self.feature_type == 'orb': self._feature_extractor = ORB_create() else: raise ValueError("Unknown feature extractor %s" % self.feature_type) if self.feature_type == 'orb': self._matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True) else: self._matcher = cv2.BFMatcher(cv2.NORM_L2, crossCheck=True) self._target_key_points = None self._target_descriptors = None @property def feature_type(self): return self._feature_type @property def target_key_points(self): return self._target_key_points @property def target_descriptors(self): return self._target_descriptors def _step(self, action): obs, reward, done, info = SimpleQuadPanda3dEnv.step(self, action) done = done or obs.get('points') is None return obs, reward, done, info def step(self, action): return ServoingEnv.step(self, action) def reset(self, state=None): self._target_key_points = None self._target_descriptors = None self._target_obs = None self._target_pos = self.get_relative_target_position() self._t = 0 return SimpleQuadPanda3dEnv.reset(self, state=state) def observe(self): obs = SimpleQuadPanda3dEnv.observe(self) assert isinstance(obs, dict) mask, depth_image, _ = self.mask_camera_sensor.observe( ) # used to filter out key points if self._target_obs is None: self._target_obs = {'target_' + k: v for (k, v) in obs.items()} self._target_key_points, self._target_descriptors = \ self._feature_extractor.detectAndCompute(self._target_obs['target_image'], mask) if self._target_key_points: key_points, descriptors = \ self._feature_extractor.detectAndCompute(obs['image'], mask if self.filter_features else None) matches = self._matcher.match(descriptors, self._target_descriptors) else: matches = False if matches: key_points_xy = np.array( [key_points[match.queryIdx].pt for match in matches]) target_key_points_xy = np.array([ self._target_key_points[match.trainIdx].pt for match in matches ]) key_points_XYZ = xy_depth_to_XYZ(self.camera_sensor.lens, key_points_xy, depth_image) target_key_points_XYZ = xy_depth_to_XYZ(self.camera_sensor.lens, target_key_points_xy, depth_image) obs['points'] = key_points_XYZ obs['target_points'] = target_key_points_XYZ else: obs['points'] = None obs['target_points'] = None obs.update(self._target_obs) return obs