camera_a.camera_id = 'cam_a'
#camera_a.pose = posemath.toMsg(PyKDL.Frame(PyKDL.Rotation.RPY(0, pi/2.0, 0), PyKDL.Vector(0, 0 ,0)))
camera_a.pose = posemath.toMsg(PyKDL.Frame(PyKDL.Vector(0, 0 ,0)))
camera_b = CameraPose()
camera_b.camera_id = 'cam_b'
#camera_b.pose = posemath.toMsg(PyKDL.Frame(PyKDL.Rotation.RPY(0,pi/2.0,0), PyKDL.Vector(0, -1, 0)))
camera_b.pose = posemath.toMsg(PyKDL.Frame(PyKDL.Vector(0, -1, 0)))
#target_1 = posemath.toMsg(PyKDL.Frame(PyKDL.Rotation.RPY(0, pi/7.0, 0), PyKDL.Vector(1, 0, 0)))
target_1 = posemath.toMsg(PyKDL.Frame(PyKDL.Vector(0, -2, 1)))
#target_2 = posemath.toMsg(PyKDL.Frame(PyKDL.Rotation.RPY(0, pi/3.0, 0), PyKDL.Vector(2, 0, 0)))
target_2 = posemath.toMsg(PyKDL.Frame(PyKDL.Vector(0, 2, 1)))
cal_estimate.cameras = [camera_a, camera_b]
cal_estimate.targets = [target_1, target_2]
print cal_estimate
# generate samples
noise = 5.0
offset = PyKDL.Frame(PyKDL.Rotation.RPY(0.1, 0.1, 0), PyKDL.Vector(0, 0.1, 0))
#P = [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0]
P = [525, 0, 319.5, 0, 0, 525, 239.5, 0, 0, 0, 1, 0]
P_mat = reshape( matrix(P, float), (3,4) )
cal_pattern = CalibrationPattern()
for c in check_points:
pnt = Point32()
pnt.x = c[0]
# read data from bag
if len(sys.argv) >= 2:
BAG = sys.argv[1]
else:
BAG = '/u/vpradeep/kinect_bags/kinect_extrinsics_2011-04-05-16-01-28.bag'
bag = rosbag.Bag(BAG)
for topic, msg, t in bag:
assert topic == 'robot_measurement'
cal_samples = [msg for topic, msg, t in bag]
# create prior
camera_poses, checkerboard_poses = init_optimization_prior.find_initial_poses(cal_samples)
cal_estimate = CalibrationEstimate()
cal_estimate.targets = [ posemath.toMsg(checkerboard_poses[i]) for i in range(len(checkerboard_poses)) ]
cal_estimate.cameras = [ CameraPose(camera_id, posemath.toMsg(camera_pose)) for camera_id, camera_pose in camera_poses.iteritems()]
# Run optimization
new_cal_estimate = estimate.enhance(cal_samples, cal_estimate)
cam_dict_list = dump_estimate.to_dict_list(new_cal_estimate.cameras)
# For now, hardcode what transforms we care about
tf_config = dict();
tf_config['camera_a'] = {'calibrated_frame':'camera_a/openni_rgb_optical_frame',
'parent_frame': 'world_frame',
'child_frame': 'camera_a/openni_camera'}
tf_config['camera_b'] = {'calibrated_frame':'camera_b/openni_rgb_optical_frame',
'parent_frame': 'world_frame',
'child_frame': 'camera_b/openni_camera'}
