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
urdf_path = os.path.join(
roslib.packages.get_pkg_dir("arm_robot_model"), "models/sensorsValues.urdf.xacro")
print 'Cal Successful! Writing result to URDF to %s' % urdf_path
bb_left_neck_tf = ros_util.matrix_to_transform(linalg.inv(new_est_cam_H_neck))
update_sensors_values_urdf(urdf_path, bb_left_neck_tf, None, None, None)
while not rospy.is_shutdown():
fcviz.update()
rospy.sleep(0.1)
print ""
print "=========================="
print "Results:"
opt.print_stats()
plt.plot(opt.errors)
plt.show()
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(frames, initial_cam_H_neck, "initial_camera_poses", initial_color, cam_mark_lines=False)
fcviz.draw_target(est_base_H_target, "estimated_target_pose", estimated_color)
fcviz.draw_frames(frames, est_cam_H_neck, "estimated_head_poses", estimated_color, cam_mark_lines=True)
# write out the resulting cal file to a urdf
urdf_path = os.path.join(roslib.packages.get_pkg_dir("arm_robot_model"), "models/sensorsValues.urdf.xacro")
print "Cal Successful! Writing result to URDF to %s" % urdf_path
bb_left_neck_tf = ros_util.matrix_to_transform(linalg.inv(est_cam_H_neck))
update_sensors_values_urdf(urdf_path, bb_left_neck_tf, None, None, None)
# for our own purposes, save the estimated transform in a shelf
cache_dir = roslib.packages.get_pkg_subdir("arm_fiducial_cal", "cache")
s = store.Store(cache_dir)
s["frames"] = frames
s["est_base_H_target"] = est_base_H_target
s["est_cam_H_neck"] = est_cam_H_neck
