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
