openpilot_comma/tools/livedm/helpers.py

import numpy as np
import cv2  # pylint: disable=import-error

def rot_matrix(roll, pitch, yaw):
  cr, sr = np.cos(roll), np.sin(roll)
  cp, sp = np.cos(pitch), np.sin(pitch)
  cy, sy = np.cos(yaw), np.sin(yaw)
  rr = np.array([[1, 0, 0], [0, cr, -sr], [0, sr, cr]])
  rp = np.array([[cp, 0, sp], [0, 1, 0], [-sp, 0, cp]])
  ry = np.array([[cy, -sy, 0], [sy, cy, 0], [0, 0, 1]])
  return ry.dot(rp.dot(rr))

def draw_pose(img, pose, loc, W=160, H=320, xyoffset=(0, 0), faceprob=0):
  rcmat = np.zeros((3, 4))
  rcmat[:, :3] = rot_matrix(*pose[0:3]) * 0.5
  rcmat[0, 3] = (loc[0]+0.5) * W
  rcmat[1, 3] = (loc[1]+0.5) * H
  rcmat[2, 3] = 1.0
  # draw nose
  p1 = np.dot(rcmat, [0, 0, 0, 1])[0:2]
  p2 = np.dot(rcmat, [0, 0, 100, 1])[0:2]
  tr = tuple([int(round(x + xyoffset[i])) for i, x in enumerate(p1)])
  pr = tuple([int(round(x + xyoffset[i])) for i, x in enumerate(p2)])
  if faceprob > 0.4:
    color = (255, 255, 0)
    cv2.line(img, tr, pr, color=(255, 255, 0), thickness=3)
  else:
    color = (64, 64, 64)
  cv2.circle(img, tr, 7, color=color)