#!/usr/bin/env python import cv2 import dlib import numpy as np import os import pickle from PIL import Image, ImageDraw # Read Image c = cv2.VideoCapture(0) # im = cv2.imread("headPose.jpg"); predictor_path = "shape_predictor_68_face_landmarks.dat" detector = dlib.get_frontal_face_detector() predictor = dlib.shape_predictor(predictor_path) screenDrawCorners = np.array([[10,60], [90, 60], [10, 110], [90, 110]]) def create_perspective_transform_matrix(src, dst): """ Creates a perspective transformation matrix which transforms points in quadrilateral ``src`` to the corresponding points on quadrilateral ``dst``. Will raise a ``np.linalg.LinAlgError`` on invalid input. """ # See: # * http://xenia.media.mit.edu/~cwren/interpolator/ # * http://stackoverflow.com/a/14178717/71522 in_matrix = [] for (x, y), (X, Y) in zip(src, dst): in_matrix.extend([ [x, y, 1, 0, 0, 0, -X * x, -X * y], [0, 0, 0, x, y, 1, -Y * x, -Y * y], ]) A = np.matrix(in_matrix, dtype=np.float) B = np.array(dst).reshape(8) af = np.dot(np.linalg.inv(A.T * A) * A.T, B) m = np.append(np.array(af).reshape(8), 1).reshape((3, 3)) print("Created transformmatrix:", src, dst, m) return m def transMatrix2(fromMatrix, toMatrix): matrix = [] for p1, p2 in zip(toMatrix, fromMatrix): matrix.append([p1[0], p1[1], 1, 0, 0, 0, -p2[0]*p1[0], -p2[0]*p1[1]]) matrix.append([0, 0, 0, p1[0], p1[1], 1, -p2[1]*p1[0], -p2[1]*p1[1]]) A = np.matrix(matrix, dtype=np.float) B = np.array(fromMatrix).reshape(8) res = np.dot(np.linalg.inv(A.T * A) * A.T, B) m = np.append(np.array(res).reshape(8), 1).reshape((3,3)) return m def create_perspective_transform(src, dst, round=False, splat_args=False): """ Returns a function which will transform points in quadrilateral ``src`` to the corresponding points on quadrilateral ``dst``:: >>> transform = create_perspective_transform( ... [(0, 0), (10, 0), (10, 10), (0, 10)], ... [(50, 50), (100, 50), (100, 100), (50, 100)], ... ) >>> transform((5, 5)) (74.99999999999639, 74.999999999999957) If ``round`` is ``True`` then points will be rounded to the nearest integer and integer values will be returned. >>> transform = create_perspective_transform( ... [(0, 0), (10, 0), (10, 10), (0, 10)], ... [(50, 50), (100, 50), (100, 100), (50, 100)], ... round=True, ... ) >>> transform((5, 5)) (75, 75) If ``splat_args`` is ``True`` the function will accept two arguments instead of a tuple. >>> transform = create_perspective_transform( ... [(0, 0), (10, 0), (10, 10), (0, 10)], ... [(50, 50), (100, 50), (100, 100), (50, 100)], ... splat_args=True, ... ) >>> transform(5, 5) (74.99999999999639, 74.999999999999957) If the input values yield an invalid transformation matrix an identity function will be returned and the ``error`` attribute will be set to a description of the error:: >>> tranform = create_perspective_transform( ... np.zeros((4, 2)), ... np.zeros((4, 2)), ... ) >>> transform((5, 5)) (5.0, 5.0) >>> transform.error 'invalid input quads (...): Singular matrix """ try: transform_matrix = create_perspective_transform_matrix(src, dst) error = None except np.linalg.LinAlgError as e: transform_matrix = np.identity(3, dtype=np.float) error = "invalid input quads (%s and %s): %s" %(src, dst, e) error = error.replace("\n", "") to_eval = "def perspective_transform(%s):\n" %( splat_args and "*pt" or "pt", ) to_eval += " res = np.dot(transform_matrix, ((pt[0], ), (pt[1], ), (1, )))\n" to_eval += " res = res / res[2]\n" if round: to_eval += " return (int(round(res[0][0])), int(round(res[1][0])))\n" else: to_eval += " return (res[0][0], res[1][0])\n" locals = { "transform_matrix": transform_matrix, } locals.update(globals()) exec to_eval in locals, locals res = locals["perspective_transform"] res.matrix = transform_matrix res.error = error return res def coordinatesToSrc(coordinates): return np.array([coordinates['tl'], coordinates['tr'],coordinates['bl'], coordinates['br']]) # coordinates of the screen boundaries if os.path.exists("coordinates.p"): coordinates = pickle.load(open("coordinates.p", "rb")) transformationMatrix = create_perspective_transform_matrix(coordinatesToSrc(coordinates), screenDrawCorners) transformationMatrix2 = transMatrix2(coordinatesToSrc(coordinates), screenDrawCorners) transform = create_perspective_transform(coordinatesToSrc(coordinates), screenDrawCorners) a = [np.array([ 1312.15541183]), np.array([ 244.56278002]), 0] # a = [np.array([ 100.15541183]), np.array([ 244.56278002]), 0] print("Coords", coordinatesToSrc(coordinates)) print("Corners:", screenDrawCorners) print("src", a) print("C", transformationMatrix) print("C2", transformationMatrix2) print("new point", np.dot(a, transformationMatrix)) print("new point 2", np.dot(a, transformationMatrix2)) print("new point 2", np.dot(transformationMatrix2, a)) print("new point 3", transform(a[0:2])) # exit() else: coordinates = {'tl': None, 'tr': None, 'bl': None, 'br': None} transformationMatrix = None while True: _, im = c.read() size = im.shape # Docs: Ask the detector to find the bounding boxes of each face. The 1 in the # second argument indicates that we should upsample the image 1 time. This # will make everything bigger and allow us to detect more faces. dets = detector(im, 1) print("Number of faces detected: {}".format(len(dets))) # We use this later for calibrating currentPoint = None currentPoints = [] if len(dets) > 0: for d in dets: shape = predictor(im, d) print(shape.part(30).x, shape.part(54)) #2D image points. If you change the image, you need to change vector image_points = np.array([ (shape.part(30).x,shape.part(30).y), # Nose tip (shape.part(8).x,shape.part(8).y), # Chin (shape.part(36).x,shape.part(36).y), # Left eye left corner (shape.part(45).x,shape.part(45).y), # Right eye right corne (shape.part(48).x,shape.part(48).y), # Left Mouth corner (shape.part(54).x,shape.part(54).y) # Right mouth corner ], dtype="double") # 3D model points. model_points = np.array([ (0.0, 0.0, 0.0), # Nose tip (0.0, -330.0, -65.0), # Chin (-225.0, 170.0, -135.0), # Left eye left corner (225.0, 170.0, -135.0), # Right eye right corne (-150.0, -150.0, -125.0), # Left Mouth corner (150.0, -150.0, -125.0) # Right mouth corner ]) # Camera internals focal_length = size[1] center = (size[1]/2, size[0]/2) camera_matrix = np.array( [[focal_length, 0, center[0]], [0, focal_length, center[1]], [0, 0, 1]], dtype = "double" ) # print ("Camera Matrix :\n {0}".format(camera_matrix)) dist_coeffs = np.zeros((4,1)) # Assuming no lens distortion (success, rotation_vector, translation_vector) = cv2.solvePnP(model_points, image_points, camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_ITERATIVE) if not success: print("Error determening PnP", success) continue print ("Rotation Vector:\n {0}".format(rotation_vector)) print ("Translation Vector:\n {0}".format(translation_vector)) # Project a 3D point (0, 0, 1000.0) onto the image plane. # We use this to draw a line sticking out of the nose (nose_end_point2D, jacobian) = cv2.projectPoints(np.array([(0.0, 0.0, 1000.0)]), rotation_vector, translation_vector, camera_matrix, dist_coeffs) for p in image_points: cv2.circle(im, (int(p[0]), int(p[1])), 3, (0,0,255), -1) p1 = ( int(image_points[0][0]), int(image_points[0][1])) p2 = ( int(nose_end_point2D[0][0][0]), int(nose_end_point2D[0][0][1])) cv2.line(im, p1, p2, (255,0,0), 2) rotMatrix = np.zeros([3,3]) cv2.Rodrigues(rotation_vector, rotMatrix, jacobian=0) # Find rotation: https://stackoverflow.com/a/15029416 rx = np.arctan2(rotMatrix[2,1], rotMatrix[2,2]) ry = np.arctan2(-rotMatrix[2,0], np.sqrt(np.square(rotMatrix[2,1]) + np.square(rotMatrix[2,2]))) rz = np.arctan2(rotMatrix[1,0],rotMatrix[0,0]) print("rotation", rx, ry, rz) ry = - np.arcsin(rotMatrix[0,2]) rx = np.arctan2(rotMatrix[1,2]/np.cos(ry), rotMatrix[2,2]/np.cos(ry)) rz = np.arctan2(rotMatrix[0,1]/np.cos(ry), rotMatrix[0,0]/np.cos(ry)) print("rotation ml", rx, ry, rz) # seems better? # draw little floorplan for x: 10 -> 50 maps to z: 0 -> 10000, x: -2000 -> 2000 mapPosX = int((translation_vector[0] + 500) / 1000 * 40) mapPosY = int((translation_vector[1] + 500) / 1000 * 40) mapPosZ = int((translation_vector[2] + 0 ) / 10000 * 40) cv2.circle(im, (mapPosZ + 10, mapPosX + 10), 2, (0,0,255), -1) cv2.circle(im, (mapPosZ + 60, mapPosY + 10), 2, (0,0,255), -1) # make it an _amazing_ stick figurine for the side view cv2.line(im, (mapPosZ + 60, mapPosY + 10), (mapPosZ + 60, mapPosY + 20), (0,0,255), 1) cv2.line(im, (mapPosZ + 60, mapPosY + 20), (mapPosZ + 55, mapPosY + 25), (0,0,255), 1) cv2.line(im, (mapPosZ + 60, mapPosY + 20), (mapPosZ + 65, mapPosY + 25), (0,0,255), 1) cv2.line(im, (mapPosZ + 60, mapPosY + 15), (mapPosZ + 55, mapPosY + 10), (0,0,255), 1) cv2.line(im, (mapPosZ + 60, mapPosY + 15), (mapPosZ + 65, mapPosY + 10), (0,0,255), 1) # draw rotation vector cv2.circle(im, (mapPosZ + 60, mapPosY + 10), 2, (0,0,255), -1) viewDirectionVector = np.dot(np.array([0.0, 0.0, 1000.0]), rotMatrix) cv2.line(im, (mapPosZ + 10, mapPosX + 10), (mapPosZ + 10 + int(viewDirectionVector[2] * 100), mapPosX + 10 + int(viewDirectionVector[0] * 100)), (255,255,0), 1) cv2.line(im, (mapPosZ + 60, mapPosY + 10), (mapPosZ + 60 + int(viewDirectionVector[2] * 100), mapPosY + 10 - int(viewDirectionVector[1] * 100)), (255,0,255), 1) # Translation vector gives position in space: # x, y z: 0,0,0 is center of camera # line: (x,y,z) = f(a) = (t1 + r1*a, t2+r2*a, t3 + r3*a) # Screen: (x,y,z) = (x,y,0) # Interesection: # x = t1 + r1 * a # y = t2 + r2 * a # z = t3 * r3 * a = 0 # => a = -t3 / r3 # substitute found a in x,y a = - translation_vector[2] / rotation_vector[2] x = translation_vector[0] + rotation_vector[0] * a y = translation_vector[1] + rotation_vector[1] * a a = - translation_vector[2] / viewDirectionVector[2] x = translation_vector[0] + viewDirectionVector[0] * a y = translation_vector[1] + viewDirectionVector[1] * a point = np.array([x,y]) currentPoint = point currentPoints.append(point) # TODO only draw nose line now, so we can change color depending whether on screen or not # processed all faces, now draw on screen: # draw little floorplan for 10 -> 50, sideplan 60 -> 100 (40x40 px) cv2.rectangle(im, (9, 9), (51, 51), (255,255,255), 1) cv2.rectangle(im, (59, 9), (101, 51), (255,255,255), 1) cv2.line(im, (10,10), (10,50), (200,200,200), 2) cv2.line(im, (60,10), (60,50), (200,200,200), 2) # screen is 16:10 cv2.rectangle(im, (9, 59), (91, 111), (255,255,255), 1) if transformationMatrix is None: cv2.putText(im, "1", (10,70), cv2.FONT_HERSHEY_PLAIN, .7, (255,255,255) if coordinates['tl'] is not None else (0,0,255)) cv2.putText(im, "2", (85,70), cv2.FONT_HERSHEY_PLAIN, .7, (255,255,255) if coordinates['tr'] is not None else (0,0,255)) cv2.putText(im, "3", (10,110), cv2.FONT_HERSHEY_PLAIN, .7, (255,255,255) if coordinates['bl'] is not None else (0,0,255)) cv2.putText(im, "4", (85,110), cv2.FONT_HERSHEY_PLAIN, .7, (255,255,255) if coordinates['br'] is not None else (0,0,255)) else: for point in currentPoints: # check if within coordinates: # dot1 = np.dot(coordinates['tl'] - point, coordinates['tl'] - coordinates['br']) # dot2 = np.dot(coordinates['bl'] - point, coordinates['tl'] - coordinates['br']) # pointIn3 = [point[0], point[1], 0] # targetPoint = np.dot(pointIn3, transformationMatrix) # print("Looking at", pointIn3, np.dot( transformationMatrix, pointIn3)) targetPoint = transform(point) print("Looking at", point, targetPoint) # cv2.circle(im, (int(targetPoint[0]), int(targetPoint[1])), 2, (0,255,0), -1) cv2.circle(im, (int(targetPoint[0]), int(targetPoint[1])), 2, (0,255,0), -1) # Display image cv2.imshow("Output", im) keyPress = cv2.waitKey(5) if keyPress==27: break elif keyPress > -1 and currentPoint is not None: if keyPress == ord('1'): coordinates['tl'] = currentPoint elif keyPress == ord('2'): coordinates['tr'] = currentPoint elif keyPress == ord('3'): coordinates['bl'] = currentPoint elif keyPress == ord('4'): coordinates['br'] = currentPoint print(coordinates.values()) pickle.dump( coordinates, open( "coordinates.p", "wb" ) ) print("Saved coordinates") if not any (x is None for x in coordinates.values()): # measured corners for corner pin # fromMatrix = np.array(coordinates.values()) # # Drawing area: # toMatrix = screenDrawCorners # matrix = [] # for p1, p2 in zip(toMatrix, fromMatrix): # matrix.append([p1[0], p1[1], 1, 0, 0, 0, -p2[0]*p1[0], -p2[0]*p1[1]]) # matrix.append([0, 0, 0, p1[0], p1[1], 1, -p2[1]*p1[0], -p2[1]*p1[1]]) # A = np.matrix(matrix, dtype=np.float) # B = np.array(fromMatrix).reshape(8) # res = np.dot(np.linalg.inv(A.T * A) * A.T, B) # transformationMatrix = np.array(res).reshape(8) transformationMatrix = create_perspective_transform_matrix(coordinatesToSrc(coordinates), screenDrawCorners) # measured corners for corner pin # fromMatrix = np.array(coordinates.values()) # # Drawing area: # toMatrix = np.array([[10,60], [90, 60], [10, 110], [90, 110]]) # print(fromMatrix, toMatrix) # print(np.linalg.inv(toMatrix)) # # matrix to transform from measured to drawed space # transformationMatrix = np.dot(fromMatrix, np.linalg.inv(toMatrix)) cv2.destroyAllWindows()