def eigen_texture(cv_image, blocksize=8, filtersize=3): gray_image = cv.cvCreateImage(cv.cvSize(cv_image.width, cv_image.height), cv.IPL_DEPTH_8U, 1) eig_tex = cv.cvCreateImage(cv.cvSize(cv_image.width * 6, cv_image.height), cv.IPL_DEPTH_32F, 1) cv.cvCvtColor(cv_image, gray_image, cv.CV_BGR2GRAY) cv.cvCornerEigenValsAndVecs(gray_image, eig_tex, blocksize, filtersize) eig_tex_np = ut.cv2np(eig_tex) eig_tex_np = np.reshape(eig_tex_np, [cv_image.height, cv_image.width, 6]) return eig_tex_np[:, :, 0:2]
def image_region(self): ''' takes images from the UTM camera at different angles. returns list of servo angles, list of images. images are numpy images. so that they can be pickled. ''' im_list = [] p_list = [] for cmd_ang in [0,30,45]: self.thok.servo.move_angle(math.radians(cmd_ang)) cvim = self.cam.get_frame() cvim = self.cam.get_frame() im_list.append(uto.cv2np(cvim,format='BGR')) p_list.append(self.thok.servo.read_angle()) self.thok.servo.move_angle(math.radians(0)) return p_list,im_list
def select_location(c, thok, angle): thok.servo.move_angle(angle) cvim = c.get_frame() cvim = c.get_frame() cvim = c.get_frame() im_angle = thok.servo.read_angle() tilt_angles = (math.radians(-20) + angle, math.radians(30) + angle) pos_list, scan_list = thok.scan(tilt_angles, speed=math.radians(10)) m = p3d.generate_pointcloud(pos_list, scan_list, math.radians(-60), math.radians(60), 0.0, -0.055) pts = mcf.utmcam0Tglobal(mcf.globalTthok0(m), im_angle) cam_params = cc.camera_parameters['mekabotUTM'] fx = cam_params['focal_length_x_in_pixels'] fy = cam_params['focal_length_y_in_pixels'] cx, cy = cam_params['optical_center_x_in_pixels'], cam_params[ 'optical_center_y_in_pixels'] cam_proj_mat = np.matrix([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) cvim, pts2d = cul.project_points_in_image(cvim, pts, cam_proj_mat) cp = cul.get_click_location(cvim) print 'Clicked location:', cp if cp == None: return None idx = cul.get_index(pts2d.T, cp) pt3d = pts[:, idx] pt_interest = mcf.globalTutmcam0(pt3d, im_angle) hl_thok0 = mcf.thok0Tglobal(pt_interest) l1, l2 = 0.0, -0.055 d = {} d['pt'] = hl_thok0 d['pos_list'], d['scan_list'] = pos_list, scan_list d['l1'], d['l2'] = l1, l2 d['img'] = uto.cv2np(cvim) ut.save_pickle(d, 'hook_plane_scan_' + ut.formatted_time() + '.pkl') return pt_interest
def prepare(self, features_k_nearest_neighbors, nonzero_indices = None, all_save_load = False, regenerate_neightborhood_indices = False): #print np.shape(self.processor.pts3d_bound), 'shape pts3d_bound' imgTmp = cv.cvCloneImage(self.processor.img) self.imNP = ut.cv2np(imgTmp,format='BGR') ###self.processor.map2d = np.asarray(self.processor.camPts_bound) #copied from laser to image mapping if features_k_nearest_neighbors == None or features_k_nearest_neighbors == False: #use range self.kdtree2d = kdtree.KDTree(self.processor.pts3d_bound.T) #print len(nonzero_indices) #print np.shape(np.asarray((self.processor.pts3d_bound.T)[nonzero_indices])) if nonzero_indices != None: print ut.getTime(), 'query ball tree for ', len(nonzero_indices), 'points' kdtree_query = kdtree.KDTree((self.processor.pts3d_bound.T)[nonzero_indices]) else: print ut.getTime(), 'query ball tree' kdtree_query = kdtree.KDTree(self.processor.pts3d_bound.T) filename = self.processor.config.path+'/data/'+self.processor.scan_dataset.id+'_sphere_neighborhood_indices_'+str(self.processor.feature_radius)+'.pkl' if all_save_load == True and os.path.exists(filename) and regenerate_neightborhood_indices == False: #if its already there, load it: print ut.getTime(), 'loading',filename self.kdtree_queried_indices = ut.load_pickle(filename) else: self.kdtree_queried_indices = kdtree_query.query_ball_tree(self.kdtree2d, self.processor.feature_radius, 2.0, 0.2) #approximate print ut.getTime(), 'queried kdtree: ',len(self.kdtree_queried_indices),'points, radius:',self.processor.feature_radius if all_save_load == True: ut.save_pickle(self.kdtree_queried_indices, filename) #make dict out of list for faster operations? (doesn't seem to change speed significantly): #self.kdtree_queried_indices = dict(zip(xrange(len(self.kdtree_queried_indices)), self.kdtree_queried_indices)) else: #experiemental: use_20_nearest_neighbors == True #TODO: exclude invalid values in get_featurevector (uncomment code there) self.kdtree2d = kdtree.KDTree(self.processor.pts3d_bound.T) self.kdtree_queried_indices = [] print ut.getTime(), 'kdtree single queries for kNN start, k=', features_k_nearest_neighbors count = 0 for point in ((self.processor.pts3d_bound.T)[nonzero_indices]): count = count + 1 result = self.kdtree2d.query(point, features_k_nearest_neighbors,0.2,2,self.processor.feature_radius) #existing = result[0][0] != np.Inf #print existing #print result[1] self.kdtree_queried_indices += [result[1]] #[existing] if count % 4096 == 0: print ut.getTime(),count print ut.getTime(), 'kdtree singe queries end' #convert to numpy array -> faster access self.kdtree_queried_indices = np.asarray(self.kdtree_queried_indices) #print self.kdtree_queried_indices #takes long to compute: #avg_len = 0 #minlen = 999999 #maxlen = 0 #for x in self.kdtree_queried_indices: # avg_len += len(x) # minlen = min(minlen, len(x)) # maxlen = max(maxlen, len(x)) #avg_len = avg_len / len(self.kdtree_queried_indices) #print ut.getTime(), "range neighbors: avg_len", avg_len, 'minlen', minlen, 'maxlen', maxlen #create HSV numpy images: # compute the hsv version of the image image_size = cv.cvGetSize(self.processor.img) img_h = cv.cvCreateImage (image_size, 8, 1) img_s = cv.cvCreateImage (image_size, 8, 1) img_v = cv.cvCreateImage (image_size, 8, 1) img_hsv = cv.cvCreateImage (image_size, 8, 3) cv.cvCvtColor (self.processor.img, img_hsv, cv.CV_BGR2HSV) cv.cvSplit (img_hsv, img_h, img_s, img_v, None) self.imNP_h = ut.cv2np(img_h) self.imNP_s = ut.cv2np(img_s) self.imNP_v = ut.cv2np(img_v) textures = texture_features.eigen_texture(self.processor.img) self.imNP_tex1 = textures[:,:,0] self.imNP_tex2 = textures[:,:,1] self.debug_before_first_featurevector = True self.generate_voi_histogram(self.processor.point_of_interest,self.processor.voi_width)
def prepare(self, features_k_nearest_neighbors, nonzero_indices=None, all_save_load=False, regenerate_neightborhood_indices=False): #print np.shape(self.processor.pts3d_bound), 'shape pts3d_bound' imgTmp = cv.cvCloneImage(self.processor.img) self.imNP = ut.cv2np(imgTmp, format='BGR') ###self.processor.map2d = np.asarray(self.processor.camPts_bound) #copied from laser to image mapping if features_k_nearest_neighbors == None or features_k_nearest_neighbors == False: #use range self.kdtree2d = kdtree.KDTree(self.processor.pts3d_bound.T) #print len(nonzero_indices) #print np.shape(np.asarray((self.processor.pts3d_bound.T)[nonzero_indices])) if nonzero_indices != None: print ut.getTime(), 'query ball tree for ', len( nonzero_indices), 'points' kdtree_query = kdtree.KDTree( (self.processor.pts3d_bound.T)[nonzero_indices]) else: print ut.getTime(), 'query ball tree' kdtree_query = kdtree.KDTree(self.processor.pts3d_bound.T) filename = self.processor.config.path + '/data/' + self.processor.scan_dataset.id + '_sphere_neighborhood_indices_' + str( self.processor.feature_radius) + '.pkl' if all_save_load == True and os.path.exists( filename) and regenerate_neightborhood_indices == False: #if its already there, load it: print ut.getTime(), 'loading', filename self.kdtree_queried_indices = ut.load_pickle(filename) else: self.kdtree_queried_indices = kdtree_query.query_ball_tree( self.kdtree2d, self.processor.feature_radius, 2.0, 0.2) #approximate print ut.getTime(), 'queried kdtree: ', len( self.kdtree_queried_indices ), 'points, radius:', self.processor.feature_radius if all_save_load == True: ut.save_pickle(self.kdtree_queried_indices, filename) #make dict out of list for faster operations? (doesn't seem to change speed significantly): #self.kdtree_queried_indices = dict(zip(xrange(len(self.kdtree_queried_indices)), self.kdtree_queried_indices)) else: #experiemental: use_20_nearest_neighbors == True #TODO: exclude invalid values in get_featurevector (uncomment code there) self.kdtree2d = kdtree.KDTree(self.processor.pts3d_bound.T) self.kdtree_queried_indices = [] print ut.getTime( ), 'kdtree single queries for kNN start, k=', features_k_nearest_neighbors count = 0 for point in ((self.processor.pts3d_bound.T)[nonzero_indices]): count = count + 1 result = self.kdtree2d.query(point, features_k_nearest_neighbors, 0.2, 2, self.processor.feature_radius) #existing = result[0][0] != np.Inf #print existing #print result[1] self.kdtree_queried_indices += [result[1]] #[existing] if count % 4096 == 0: print ut.getTime(), count print ut.getTime(), 'kdtree singe queries end' #convert to numpy array -> faster access self.kdtree_queried_indices = np.asarray( self.kdtree_queried_indices) #print self.kdtree_queried_indices #takes long to compute: #avg_len = 0 #minlen = 999999 #maxlen = 0 #for x in self.kdtree_queried_indices: # avg_len += len(x) # minlen = min(minlen, len(x)) # maxlen = max(maxlen, len(x)) #avg_len = avg_len / len(self.kdtree_queried_indices) #print ut.getTime(), "range neighbors: avg_len", avg_len, 'minlen', minlen, 'maxlen', maxlen #create HSV numpy images: # compute the hsv version of the image image_size = cv.cvGetSize(self.processor.img) img_h = cv.cvCreateImage(image_size, 8, 1) img_s = cv.cvCreateImage(image_size, 8, 1) img_v = cv.cvCreateImage(image_size, 8, 1) img_hsv = cv.cvCreateImage(image_size, 8, 3) cv.cvCvtColor(self.processor.img, img_hsv, cv.CV_BGR2HSV) cv.cvSplit(img_hsv, img_h, img_s, img_v, None) self.imNP_h = ut.cv2np(img_h) self.imNP_s = ut.cv2np(img_s) self.imNP_v = ut.cv2np(img_v) textures = texture_features.eigen_texture(self.processor.img) self.imNP_tex1 = textures[:, :, 0] self.imNP_tex2 = textures[:, :, 1] self.debug_before_first_featurevector = True self.generate_voi_histogram(self.processor.point_of_interest, self.processor.voi_width)