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
#seeds = np.array([ 0.003, 0.1 , -0.02, 89.8, 89.8, 90.0, 0])
seeds = np.array([-0.087, 0.105, 0.01, 89.8, 89.8, 90.0, 0])
#deltas = np.array([0.005, 0.005, 0.005, 0.1, 0.1, 0.1, 0.1])
#seeds = np.array([0.061, 0.032, -0.035, 0.8, 0.9, -1.7, 3.1 ])
deltas = np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1, 0.1])
#self.cam_names = ['Ry_0', 'Rz_0', 'Rx_-90', 'Rz_-90', 'dx', 'dy', 'dz']
names = ['x_disp', 'y_disp', 'z_disp', 'rotX', 'rotZ', 'rotX', 'rotZ']
img = hg.cvLoadImage(image_filename)
raw_laser_scans = ut.load_pickle(point_cloud_filename)
#if raw_laser_scans.__class__ == ().__class__:
poses, scans = raw_laser_scans['laserscans'][0]
points_cloud_laser = p3d.generate_pointcloud(poses,
scans,
math.radians(-180),
math.radians(180),
0,
.035,
max_dist=5,
min_dist=.2)
#else:
# points_cloud_laser = raw_laser_scans
import webcam_config as cc
cp = cc.webcam_parameters['DesktopWebcam']
fx = cp['focal_length_x_in_pixels']
fy = cp['focal_length_y_in_pixels']
cam_proj_mat = np.matrix([[fx, 0, 0, 0], [0, fy, 0, 0], [0, 0, 1, 0]])
cam_centers = (cp['optical_center_x_in_pixels'],
cp['optical_center_y_in_pixels'])
cam_centers = ( cp['optical_center_x_in_pixels'], cp['optical_center_y_in_pixels'] )
#take image and scan
import scanner
import configuration
#id = '2009Nov04_144041'
id = '2009Nov04_135319'
cfg = configuration.configuration('/home/martin/robot1_data/usr/martin/laser_camera_segmentation/labeling')
img = hg.cvLoadImage(cfg.path + '/data/' + id + '_image.png')
thok_dict = ut.load_pickle(cfg.path + '/data/' + id + '_laserscans.pkl')
#cfg = configuration.configuration('/home/martin/robot1_data/usr/martin/laser_camera_segmentation/calib')
#cfg.webcam_id = 0
#sc = scanner.scanner(cfg)
#sc.capture_and_save('calib', False)
#img = hg.cvLoadImage('/home/martin/robot1_data/usr/martin/laser_camera_segmentation/calib/data/calib_image.png')
#thok_dict = ut.load_pickle('/home/martin/robot1_data/usr/martin/laser_camera_segmentation/calib/data/calib_laserscans.pkl')
poses, scans = thok_dict['laserscans'][0]
points_cloud_laser = p3d.generate_pointcloud(poses, scans, math.radians(-180), math.radians(180),
0, .035, max_dist=5.0, min_dist=.1)
c = laser_cam_callib.Callib(cameraTlaser, seeds, deltas, names, points_cloud_laser, img, cam_proj_mat, cam_centers,1, id)
while not c.reDraw():
tmp = 1
pygame.quit()
opt, args = p.parse_args()
pts_pkl_fname = opt.pts_pkl
dict_pkl_fname = opt.dict_pkl
save_cloud_flag = opt.save_cloud
max_pan_angle = opt.max_pan_angle
max_dist = opt.max_dist
if pts_pkl_fname != None:
pts = ut.load_pickle(pts_pkl_fname)
elif dict_pkl_fname != None:
import tilting_hokuyo.processing_3d as p3d
dict = ut.load_pickle(dict_pkl_fname)
pts = p3d.generate_pointcloud(dict['pos_list'],dict['scan_list'],
math.radians(-max_pan_angle),
math.radians(max_pan_angle),
dict['l1'],dict['l2'],
min_tilt=math.radians(-90),max_tilt=math.radians(90))
else:
print 'Specify either a pts pkl or a dict pkl (-c or -f)'
print 'Exiting...'
sys.exit()
dist_mat = ut.norm(pts)
idxs = np.where(dist_mat<max_dist)[1]
print 'pts.shape', pts.shape
pts = pts[:,idxs.A1]
print 'pts.shape', pts.shape
if save_cloud_flag:
ut.save_pickle(pts,'numpy_pc_'+ut.formatted_time()+'.pkl')
opt, args = p.parse_args()
pts_pkl_fname = opt.pts_pkl
dict_pkl_fname = opt.dict_pkl
save_cloud_flag = opt.save_cloud
max_pan_angle = opt.max_pan_angle
max_dist = opt.max_dist
if pts_pkl_fname != None:
pts = ut.load_pickle(pts_pkl_fname)
elif dict_pkl_fname != None:
import tilting_hokuyo.processing_3d as p3d
dict = ut.load_pickle(dict_pkl_fname)
pts = p3d.generate_pointcloud(dict['pos_list'],dict['scan_list'],
math.radians(-max_pan_angle),
math.radians(max_pan_angle),
dict['l1'],dict['l2'],
min_tilt=math.radians(-90),max_tilt=math.radians(90))
else:
print 'Specify either a pts pkl or a dict pkl (-c or -f)'
print 'Exiting...'
sys.exit()
dist_mat = ut.norm(pts)
idxs = np.where(dist_mat<max_dist)[1]
print 'pts.shape', pts.shape
pts = pts[:,idxs.A1]
print 'pts.shape', pts.shape
if save_cloud_flag:
ut.save_pickle(pts,'numpy_pc_'+ut.formatted_time()+'.pkl')
