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