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_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()
spread = dist_from_line
if spread < 0.1:
print 'Skipping frame %d (spread is only %f)' % (f_i, spread)
# train gp based on head joint angles
head_joint_angles = np.array(f.joint_states.position)
gp_training_inputs.append(head_joint_angles)
opt = FCSingleFrameOpt(params.tf_target_points, est_base_H_target, est_cam_H_neck, f)
est_params = opt.optimize()
gp_training_outputs.append(est_params)
# project points using corrected transform
cam_H_ccam = pm.params_to_matrix(est_params)
base_H_ccam = np.dot(base_H_cam, cam_H_ccam)
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, corrected_color, corrected_bf_marker_points)
fcviz.viz.add(m)
fcviz.update()
print 'Frame %d: n_markers=%d spread=%.2f: %s -> %s' % (
f.frame_id, len(f.visible_markers), spread, ' '.join(['%6.3f' % a for a in head_joint_angles]),
' '.join(['%6.3f' % p for p in est_params]))
upright_frames.append(f)
gp_training_inputs = np.array(gp_training_inputs)
gp_training_outputs = np.array(gp_training_outputs)
