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) # write out the resulting cal file to a urdf
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 est_bf_target_points = geom.transform_points(params.tf_target_points, est_base_H_target)