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 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)
