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 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()
