parent
3859d62003
commit
9a82d937a7
2 changed files with 0 additions and 108 deletions
@ -1,29 +0,0 @@ |
|||||||
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) |
|
@ -1,79 +0,0 @@ |
|||||||
#!/usr/bin/env python3 |
|
||||||
import os |
|
||||||
import argparse |
|
||||||
import pygame # pylint: disable=import-error |
|
||||||
import numpy as np |
|
||||||
import cv2 # pylint: disable=import-error |
|
||||||
|
|
||||||
from cereal import log |
|
||||||
import cereal.messaging as messaging |
|
||||||
|
|
||||||
from helpers import draw_pose |
|
||||||
|
|
||||||
if __name__ == "__main__": |
|
||||||
|
|
||||||
os.environ["ZMQ"] = "1" |
|
||||||
|
|
||||||
parser = argparse.ArgumentParser(description='Sniff a communcation socket') |
|
||||||
parser.add_argument('--addr', default='192.168.5.11') |
|
||||||
args = parser.parse_args() |
|
||||||
|
|
||||||
messaging.context = messaging.Context() |
|
||||||
|
|
||||||
poller = messaging.Poller() |
|
||||||
|
|
||||||
m = 'driverMonitoring' |
|
||||||
messaging.sub_sock(m, poller, addr=args.addr) |
|
||||||
|
|
||||||
pygame.init() |
|
||||||
pygame.display.set_caption('livedm') |
|
||||||
screen = pygame.display.set_mode((320, 640), pygame.DOUBLEBUF) |
|
||||||
camera_surface = pygame.surface.Surface((160, 320), 0, 24).convert() |
|
||||||
|
|
||||||
while 1: |
|
||||||
polld = poller.poll(1000) |
|
||||||
for sock in polld: |
|
||||||
msg = sock.receive() |
|
||||||
evt = log.Event.from_bytes(msg) |
|
||||||
|
|
||||||
faceProb = np.array(evt.driverMonitoring.faceProb) |
|
||||||
faceOrientation = np.array(evt.driverMonitoring.faceOrientation) |
|
||||||
facePosition = np.array(evt.driverMonitoring.facePosition) |
|
||||||
|
|
||||||
print(faceProb) |
|
||||||
# print(faceOrientation) |
|
||||||
# print(facePosition) |
|
||||||
faceOrientation[1] *= -1 |
|
||||||
facePosition[0] *= -1 |
|
||||||
|
|
||||||
img = np.zeros((320, 160, 3)) |
|
||||||
if faceProb > 0.4: |
|
||||||
cv2.putText(img, 'you', (int(facePosition[0]*160+40), int(facePosition[1]*320+110)), cv2.FONT_ITALIC, 0.5, (255, 255, 0)) |
|
||||||
cv2.rectangle(img, (int(facePosition[0]*160+40), int(facePosition[1]*320+120)), |
|
||||||
(int(facePosition[0]*160+120), int(facePosition[1]*320+200)), (255, 255, 0), 1) |
|
||||||
|
|
||||||
not_blink = evt.driverMonitoring.leftBlinkProb + evt.driverMonitoring.rightBlinkProb < 1 |
|
||||||
|
|
||||||
if evt.driverMonitoring.leftEyeProb > 0.6: |
|
||||||
cv2.line(img, (int(facePosition[0]*160+95), int(facePosition[1]*320+140)), |
|
||||||
(int(facePosition[0]*160+105), int(facePosition[1]*320+140)), (255, 255, 0), 2) |
|
||||||
if not_blink: |
|
||||||
cv2.line(img, (int(facePosition[0]*160+99), int(facePosition[1]*320+143)), |
|
||||||
(int(facePosition[0]*160+101), int(facePosition[1]*320+143)), (255, 255, 0), 2) |
|
||||||
|
|
||||||
if evt.driverMonitoring.rightEyeProb > 0.6: |
|
||||||
cv2.line(img, (int(facePosition[0]*160+55), int(facePosition[1]*320+140)), |
|
||||||
(int(facePosition[0]*160+65), int(facePosition[1]*320+140)), (255, 255, 0), 2) |
|
||||||
if not_blink: |
|
||||||
cv2.line(img, (int(facePosition[0]*160+59), int(facePosition[1]*320+143)), |
|
||||||
(int(facePosition[0]*160+61), int(facePosition[1]*320+143)), (255, 255, 0), 2) |
|
||||||
|
|
||||||
else: |
|
||||||
cv2.putText(img, 'you not found', (int(facePosition[0]*160+40), int(facePosition[1]*320+110)), cv2.FONT_ITALIC, 0.5, (64, 64, 64)) |
|
||||||
draw_pose(img, faceOrientation, facePosition, |
|
||||||
W=160, H=320, xyoffset=(0, 0), faceprob=faceProb) |
|
||||||
|
|
||||||
pygame.surfarray.blit_array(camera_surface, img.swapaxes(0, 1)) |
|
||||||
camera_surface_2x = pygame.transform.scale2x(camera_surface) |
|
||||||
screen.blit(camera_surface_2x, (0, 0)) |
|
||||||
pygame.display.flip() |
|
Loading…
Reference in new issue