def draw_head_pose(self, neck_H_base, cam_H_neck, ns, color):
base_H_neck = linalg.inv(neck_H_base)
neck_H_cam = linalg.inv(cam_H_neck)
self.draw_transform(base_H_neck, scale=0.1, ns=ns, idnum=0)
cam_H_base = np.dot(cam_H_neck, neck_H_base)
base_H_cam = linalg.inv(cam_H_base)
self.draw_transform(base_H_cam, scale=0.04, ns=ns, idnum=3)
# draw a line from neck to camera
b_neck_origin = geom.transform_points(np.zeros(3), base_H_neck)
b_cam_origin = geom.transform_points(np.zeros(3), base_H_cam)
m = markers.line_list('/BASE', ns, 7, 0.002, color, [b_neck_origin, b_cam_origin])
self.viz.add(m)
self.viz.update()
def __init__(self, tf_target_points, base_H_target, cam_H_neck, frame):
self.tf_target_points = tf_target_points
self.base_H_target = base_H_target
self.cam_H_neck = cam_H_neck
self.frame = frame
self.bf_target_points = geom.transform_points(self.tf_target_points, self.base_H_target)
self.pm = FCParametrizer()
def draw_observed_points(self, frame, cam_H_neck, ns, color, cam_mark_lines=False):
cf_points = np.array([cf_p for (marker_i, cf_p) in frame.visible_markers])
base_H_cam = np.dot(linalg.inv(frame.neck_H_base), linalg.inv(cam_H_neck))
bf_points = geom.transform_points(cf_points, base_H_cam)
m = markers.points('/BASE', ns, 0, 0.003, color, bf_points)
self.viz.add(m)
# draw lines from the camera to the points it sees
if cam_mark_lines:
bf_cam_origin = geom.transform_points(np.zeros(3), base_H_cam)
line_points = []
for bf_p in bf_points:
line_points.append(bf_cam_origin)
line_points.append(bf_p)
m = markers.line_list('/BASE', ns, 7, 0.001, color, line_points)
self.viz.add(m)
self.viz.update()
def draw_transform(self, H, scale, ns, idnum):
b_origin = geom.transform_points(np.array((0., 0., 0.)), H)
b_ivec = geom.transform_vectors(np.array((1., 0., 0.)), H)
b_jvec = geom.transform_vectors(np.array((0., 1., 0.)), H)
b_kvec = geom.transform_vectors(np.array((0., 0., 1.)), H)
m_list = markers.axes(
'/BASE', ns, idnum, b_origin, b_ivec, b_jvec, b_kvec, scale, 0.05*scale)
for m in m_list:
self.viz.add(m)
self.viz.update()
def calc_error(self, params):
#params[:2] = 0.0
cam_H_ccam = self.pm.params_to_matrix(params)
neck_H_ccam = np.dot(linalg.inv(self.cam_H_neck), cam_H_ccam)
err = 0.0
num_corr = 0
base_H_ccam = np.dot(linalg.inv(self.frame.neck_H_base), neck_H_ccam)
for m_i, cf_p in self.frame.visible_markers:
# position of marker in base frame as seen by camera
bf_p = geom.transform_points(cf_p, base_H_ccam)
# position of marker in base frame based on target position
bf_p_target = self.bf_target_points[m_i]
# error
err += linalg.norm(bf_p - bf_p_target)
num_corr += 1
err /= num_corr
self.errors.append(err)
#print len(self.errors), ':', params, ':', '%.3f' % err
return err
def draw_target(self, base_H_target, ns='target_points', color=(0.0, 0.0, 1.0, 1.0)):
bf_target_points = geom.transform_points(self.tf_target_points, base_H_target)
m = markers.points('/BASE', ns, 0, 0.007, color, bf_target_points)
self.viz.add(m)
self.viz.update()
params = FCParams()
rospy.init_node('FiducialCal')
rospy.sleep(1.0)
fcviz = FCViz('/FiducialCal/markers', params.tf_target_points)
# 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)
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)
# use the camera to neck transform from the last frame (should be the same for all frames though)
initial_cam_H_neck = linalg.inv(ros_util.transform_to_matrix(msg.cam_transform_head))
# run the optimization
for f_i in range(len(frames)):
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..."
opt = FCOptimizer(frames, params.tf_target_points, params.initial_base_H_target, initial_cam_H_neck)
est_base_H_target, est_cam_H_neck = opt.optimize()
est_bf_target_points = geom.transform_points(params.tf_target_points, est_base_H_target)
print "Target average z=%f" % est_bf_target_points[:, 2].mean()
# print np.dot(linalg.inv(initial_cam_H_neck), est_cam_H_neck)
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)
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)
average_table_z = est_bf_target_points[:,2].mean()
print 'Target average z=%f' % average_table_z
opt.print_stats()
# write out the resulting cal file to the urdf
urdf_path = os.path.join(
roslib.packages.get_pkg_dir("arm_robot_model"), "models/sensorsValues.urdf.xacro")
print 'Cal Successful! Writing fixed offset 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)
# write out the fixed offset correction
fixed_bb_offset_path = os.path.join(roslib.packages.get_pkg_dir("arm_fiducial_cal"), "calib/fixed_offset.txt")
print 'Cal Successful! Writing fixed offset result to %s' % fixed_bb_offset_path
print 'Notation is x y z qw qx qy qz.'
estimated_color = (1.0, 1.0, 0.0, 1.0)
gp_color = (0.0, 1.0, 0.0, 1.0)
params = FCParams()
rospy.init_node('FiducialCal')
rospy.sleep(1.0)
fcviz = FCViz('/FiducialCal/markers', params.tf_target_points)
fcviz.draw_target(est_base_H_target, 'estimated_target_pose', true_color)
for f_i, f in enumerate(upright_frames):
# use GP to predict actual base to camera transform
cam_H_base = np.dot(est_cam_H_neck, f.neck_H_base)
base_H_cam = linalg.inv(cam_H_base)
base_H_ccam = correcter.get_corrected_transform(base_H_cam)
# project points using corrected transform
cf_marker_points = np.array([cf_p for (m_i, cf_p) in f.visible_markers])
bf_marker_points = geom.transform_points(cf_marker_points, base_H_cam)
m = markers.points('/BASE', 'points_%d' % f_i, 0, 0.003, estimated_color, bf_marker_points)
fcviz.viz.add(m)
corrected_bf_marker_points = geom.transform_points(cf_marker_points, base_H_ccam)
m = markers.points('/BASE', 'corrected_points_%d' % f_i, 0, 0.003, gp_color, corrected_bf_marker_points)
fcviz.viz.add(m)
fcviz.update()
while not rospy.is_shutdown():
fcviz.update()
rospy.sleep(0.1)
