table_ymax += .0375 table_xmin -= .0375 table_ymin -= .0375 table_bounds_path = os.path.join( roslib.packages.get_pkg_subdir("arm_fiducial_cal", "calib"), "table_bounds.txt") table_bounds_f = open(table_bounds_path, 'w') print >> table_bounds_f, ' '.join(['%0.3f' % v for v in [ table_xmin, table_xmax, table_ymin, table_ymax, table_zmin, table_zmax]]) table_bounds_f.close() print '' print '====================================================================' print 'Computing GP head correction' print '====================================================================' pm = FCParametrizer() gp_training_inputs = [] gp_training_outputs = [] upright_frames = [] fcviz.draw_target(est_base_H_target, 'estimated_target_pose', true_color) for f_i, f in enumerate(frames): bf_head_origin = geom.transform_points(np.zeros(3), linalg.inv(f.neck_H_base)) if len(f.visible_markers) < 6: print 'Skipping frame %d (only %d markers visible)' % (f_i, len(f.visible_markers)) continue cam_H_base = np.dot(est_cam_H_neck, f.neck_H_base) base_H_cam = linalg.inv(cam_H_base) cf_marker_points = np.array([cf_p for (m_i, cf_p) in f.visible_markers])
cache_dir = roslib.packages.get_pkg_subdir('arm_fiducial_cal', 'cache') s = store.Store(cache_dir) est_cam_H_neck = s['est_cam_H_neck'] est_base_H_target = s['est_base_H_target'] frames = s['frames'] true_color = (1.0, 0.0, 1.0, 1.0) initial_color = (0.0, 1.0, 1.0, 1.0) estimated_color = (1.0, 1.0, 0.0, 1.0) gp_color = (0.0, 1.0, 0.0, 1.0) rospy.init_node('FiducialCal') fcviz = FCViz('/FiducialCal/markers', params.tf_target_points) fcviz.draw_target(est_base_H_target, 'estimated_target_pose', true_color) pm = FCParametrizer() gp_training_inputs = [] gp_training_outputs = [] upright_frames = [] for f_i, f in enumerate(frames): bf_head_origin = geom.transform_points(np.zeros(3), linalg.inv(f.neck_H_base)) # hack to filter out poses where the lower pan-tilt isn't straight up if bf_head_origin[2] < 1.7: print 'Skipping frame %d (pan-tilt not in upright pose)' % f_i continue if len(f.visible_markers) < 4: print 'Skipping frame %d (only %d markers visible)' % (f_i, len(f.visible_markers)) continue