s = store.Store(cache_dir)
frames = s['frames']
est_base_H_target = s['est_base_H_target']
est_cam_H_neck = s['est_cam_H_neck']
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
upright_frames.append(f)
opt = FCOptimizer(frames, params.tf_target_points, est_base_H_target, est_cam_H_neck, fix_target_transform=True,
W=np.diag([1.0, 1.0, 10.0]))
new_est_base_H_target, new_est_cam_H_neck = opt.optimize()
opt.print_stats()
print np.dot(linalg.inv(new_est_cam_H_neck), est_cam_H_neck)
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)
fcviz.draw_frames(upright_frames, est_cam_H_neck, 'new_initial_camera_poses', initial_color, cam_mark_lines=False)
fcviz.draw_target(est_base_H_target, 'estimated_target_pose', true_color)
fcviz.draw_frames(upright_frames, new_est_cam_H_neck, 'new_estimated_head_poses', estimated_color, cam_mark_lines=False)
markers_i = set([m_k for (m_k, p) in frames[f_i].visible_markers])
print 'Markers visible in frame %d:' % f_i, sorted(markers_i)
print ''
print '===================================================================='
print 'Optimizing 6DOF transforms'
print '===================================================================='
tf_target_points = params.tf_target_points
est_base_H_target = params.initial_base_H_target
est_cam_H_neck = params.initial_cam_H_neck
for opt_i in range(2):
# optimize only the base to target transform
print ''
print 'Optimizing base to target transform'
opt = FCOptimizer(frames, tf_target_points, est_base_H_target, est_cam_H_neck, opt_frame='target')
est_base_H_target, est_cam_H_neck = opt.optimize()
# assuming the base to target transform we just computed is correct, optimize
# the top-of-pan-tilt to left bumblebee transform
print ''
print 'Optimizing camera to top-of-pan-tilt transform'
opt = FCOptimizer(frames, tf_target_points, est_base_H_target, est_cam_H_neck, opt_frame='cam')
est_base_H_target, est_cam_H_neck = opt.optimize()
print ''
print '===================================================================='
print 'Calibration Results:'
print '===================================================================='
# print some statisticas about the optimization
