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