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 __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
def __init__(self,K,nfeatures=5000,norm=NORM_HAMMING,crosscheck=False): self.orb = ORB_create(nfeatures) self.bfm = BFMatcher(norm,crosscheck) self.orb_mask = None self.last = None self.K = K self.Kinv = inv(self.K) self.f_est_avg = []
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 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 ORBDetector(Detector): def __init__(self): Detector.__init__(self) self.ORB = ORB_create() def __repr__(self): return "ORBDetector(ORB={ORB})".format(ORB=self.ORB) def detect(self, image): keypoints = self.ORB.detect(image.cv_image, None) keypoints, descriptors = self.ORB.compute(image.cv_image, keypoints) return keypoints, descriptors
def image2descriptors(imlist, descriptor='ORB'): """ For each image in the imlist list extract the given descriptor (ORB and SIFT supported) Return the lists : [keypoint], [descriptor] """ from cv2 import imread desc = [] key = [] if(descriptor=='ORB'): from cv2 import ORB_create for imname in imlist: im = imread(imname) k, d = ORB_create().detectAndCompute(im, None) desc.append(d) key.append(k) if(descriptor=='BRISK'): from cv2 import BRISK_create for imname in imlist: im = imread(imname) k, d = BRISK_create().detectAndCompute(im, None) desc.append(d) key.append(k) elif(descriptor=="SIFT"): from cv2.xfeatures2d import SIFT_create for imname in imlist: im = imread(imname) k, d = SIFT_create().detectAndCompute(im, None) desc.append(d) key.append(k) else: print 'Desc is not equal to ORB or to SIFT' print 'bordel' return key, desc
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 AverageCrypt(intLength=32, intCamLoc=0): try: import numpy as np from datetime import datetime from cv2 import (FastFeatureDetector_create, ORB_create, SimpleBlobDetector_create, VideoCapture, waitKey, destroyAllWindows, CAP_DSHOW) except: print("Imports failed") #initialise camera try: cam = VideoCapture(intCamLoc, CAP_DSHOW) except: cam = "Failed" intCount = 0 intPwdLength = intLength strPassword = "" fast = FastFeatureDetector_create() orb = ORB_create() blob = SimpleBlobDetector_create() print(datetime.now()) for intCount in range(0, intPwdLength): #collect frame ret, frame = cam.read() #frame = cv.resize(frame, (320, 480)) features = fast.detect(frame, None) #split channels Ch0 = frame[:, :, 0] Ch1 = frame[:, :, 1] Ch2 = frame[:, :, 2] #average frames intCh0 = np.mean(Ch0) intCh1 = np.mean(Ch1) intCh2 = np.mean(Ch2) #initialise seed # R G B intSeed = int(intCh0) << 32 | int(intCh1) << 16 | int(intCh2) << 0 letter = 32 + ((intSeed % 126) - 32) #add letter to array strPassword += str(chr(letter)) waitKey(1) cam.release() destroyAllWindows() return strPassword
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)
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
try: cam = VideoCapture(0) except: cam = "Failed" intCount = 0 intPwdLength = 32 strPassword = "" strFileName = "./Results/Result{0}.png".format(datetime.now()) strFileName = strFileName.replace(":", "") strFileName = strFileName.replace(" ", "") print(strFileName) fast = FastFeatureDetector_create() orb = ORB_create() blob = SimpleBlobDetector_create() arryPlotX = np.zeros(126) arryPlotY = np.zeros(126) arryFastStdDev = np.zeros(intPwdLength) arryOrbStdDev = np.zeros(intPwdLength) arryBlobStdDev = np.zeros(intPwdLength) for i in range(0, 126): arryPlotY[i] = i print(datetime.now()) for intCount in range(0, intPwdLength):
def test_ORB(self): from cv2 import ORB_create test_orb = ORB_create()
class KPExtractor(object): def __init__(self,K,nfeatures=5000,norm=NORM_HAMMING,crosscheck=False): self.orb = ORB_create(nfeatures) self.bfm = BFMatcher(norm,crosscheck) self.orb_mask = None self.last = None self.K = K self.Kinv = inv(self.K) self.f_est_avg = [] def normalize(self,pts): return dot(self.Kinv,self.add_ones(pts).T).T[:, 0:2] def denormalize(self,pt): ret = dot(self.K, array([pt[0],pt[1],1.0]).T) ret /= ret[2] return int(round(ret[0])),int(round(ret[1])),int(round(ret[2])) def build_orb_mask(self,frame): h,w= frame.shape c = 1 self.orb_mask = zeros((h,w,c),dtype=uint8) rectangle(self.orb_mask,(0,0),(w,int(h*0.75)),(255,255,255),-1) # self.orb_mask = cvtColor(self.orb_mask,COLOR_BGR2GRAY) def extractRt(self,E): U,w,Vt = svd(E) assert det(U) > 0 if det(Vt) < 0: Vt *= -1.0 #Find R and T from Hartleyy and Zisserman W = mat([[0,-1,0],[1,0,0],[0,0,1]],dtype=float) R = dot(dot(U,W),Vt) if sum(R.diagonal()) < 0: R = dot(dot(U,W.T),Vt) t = U[:,2] Rt = concatenate([R,t.reshape(3,1)],axis=1) return Rt # [x,y] to [x,y,1] def add_ones(self,x): return concatenate([x, ones((x.shape[0],1))],axis=1) def extract(self,frame,maxCorners=5000,qualityLevel=.01,minDistance=3): #Detect feats = goodFeaturesToTrack(frame,maxCorners,qualityLevel,minDistance)#,mask=self.orb_mask) #Extract kps = [KeyPoint(x=f[0][0],y=f[0][1],_size=20) for f in feats] kps,des = self.orb.compute(frame,kps) #Match ret = [] if self.last is not None: #THE QUERY IMAGE IS THE ACTUAL IMAGE & #THE TRAIN IMAGE IS THE LAST IMAGE #U STOUPID KUNT matches = self.bfm.knnMatch(des,self.last['des'],k=2) for m,n in matches: if m.distance < 0.7 * n.distance: ret.append((kps[m.queryIdx].pt,self.last['kps'][m.trainIdx].pt)) #filter Rt = None if len(ret) > 0: ret = array(ret) #Normalize ret[:,0,:] = self.normalize(ret[:,0,:]) ret[:,1,:] = self.normalize(ret[:,1,:]) try: # print(f"{len(ret)=}, {ret[:,0].shape=}, {ret[:,1].shape=}") model, inliers = ransac((ret[:, 0],ret[:, 1]), #FundamentalMatrixTransform, EssentialMatrixTransform, min_samples=8, residual_threshold=0.005, max_trials=200) ret = ret[inliers] Rt = self.extractRt(model.params) except: pass self.last = {'kps': kps,'des':des} return ret,Rt #warp is pattern for now def homography(self,last_puzzle,puzzle,warp,maxCorners=5000,qualityLevel=.01,minDistance=3): #Detect f_feats = goodFeaturesToTrack(frame,maxCorners,qualityLevel,minDistance)#,mask=self.orb_mask) #Extract f_kps = [KeyPoint(x=f[0][0],y=f[0][1],_size=20) for f in f_feats] f_kps,f_des = self.orb.compute(frame,f_kps) #Detect w_feats = goodFeaturesToTrack(warp,maxCorners,qualityLevel,minDistance)#,mask=self.orb_mask) #Extract w_kps = [KeyPoint(x=f[0][0],y=f[0][1],_size=20) for f in w_feats] w_kps,w_des = self.orb.compute(warp,w_kps) #Match ret = [] # #THE QUERY IMAGE IS THE ACTUAL IMAGE & # #THE TRAIN IMAGE IS THE LAST IMAGE # #U STOUPID KUNT matches = self.bfm.knnMatch(w_des,f_des,k=2) for m,n in matches: # if m.distance <= 1 * n.distance: ret.append((w_kps[m.queryIdx].pt,f_kps[m.trainIdx].pt)) #filter Rt = None H, mask, warp_pts, orig_pts = None,None,None,None if len(ret) > 0: warp_pts = float32([r[0] for r in ret]).reshape(-1,1,2) orig_pts = float32([r[1] for r in ret]).reshape(-1,1,2) H,mask = findHomography(warp_pts,orig_pts,RANSAC,5.0) return H,mask,warp_pts,orig_pts def project_matrix(self,homography): homography *= (-1) rot_and_transl = dot(self.Kinv, homography) col_1 = rot_and_transl[:, 0] col_2 = rot_and_transl[:, 1] col_3 = rot_and_transl[:, 2] # normalise vectors l = sqrt(norm(col_1, 2) * norm(col_2, 2)) rot_1 = col_1 / l rot_2 = col_2 / l translation = col_3 / l # compute the orthonormal basis c = rot_1 + rot_2 p = cross(rot_1, rot_2) d = cross(c, p) rot_1 = dot(c / norm(c, 2) + d / norm(d, 2), 1 / sqrt(2)) rot_2 = dot(c / norm(c, 2) - d / norm(d, 2), 1 / sqrt(2)) rot_3 = cross(rot_1, rot_2) # finally, compute the 3D projection matrix from the model to the current frame projection = stack((rot_1, rot_2, rot_3, translation)).T return dot(self.K, projection)
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
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")
def __init__(self): Detector.__init__(self) self.ORB = ORB_create()
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)