From 1b049b9e5abcf08acd38c0241a35c9f1aa3e1c52 Mon Sep 17 00:00:00 2001 From: Ruben Date: Thu, 26 Apr 2018 13:12:51 +0200 Subject: [PATCH] Rudimentary visualisation --- head_pose.py | 146 +++++++++++++++++++++++++++++---------------------- helpers.py | 101 +++++++++++++++++++++++++++++++++++ test.py | 20 +++++++ 3 files changed, 203 insertions(+), 64 deletions(-) create mode 100644 helpers.py create mode 100644 test.py diff --git a/head_pose.py b/head_pose.py index 9bfd850..e2be3df 100644 --- a/head_pose.py +++ b/head_pose.py @@ -5,9 +5,14 @@ import dlib import numpy as np import os import pickle - -from PIL import Image, ImageDraw +import logging +from scipy.ndimage.filters import gaussian_filter +import Tkinter +from PIL import Image, ImageDraw,ImageTk +logging.basicConfig( format='%(asctime)-15s %(name)s %(levelname)s: %(message)s' ) +logger = logging.getLogger(__name__) + # Read Image c = cv2.VideoCapture(0) # im = cv2.imread("headPose.jpg"); @@ -20,6 +25,10 @@ predictor = dlib.shape_predictor(predictor_path) screenDrawCorners = np.array([[10,60], [90, 60], [10, 110], [90, 110]]) +# metrics matrix +metricsSize = [1920,1080] +metrics = np.zeros(metricsSize) +screenDrawCorners = np.array([[0,0], [1919,0], [0, 1079], [1919,1079]]) def create_perspective_transform_matrix(src, dst): """ Creates a perspective transformation matrix which transforms points @@ -42,24 +51,10 @@ def create_perspective_transform_matrix(src, dst): 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) + logger.info("Created transformmatrix: src %s dst %s m %s", 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 - - - +# got this amazing thing from here: https://stackoverflow.com/a/24088499 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``:: @@ -139,24 +134,23 @@ def coordinatesToSrc(coordinates): # 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])) + logger.info("Loaded coordinates: %s", coordinatesToSrc(coordinates)) + logger.debug("Corners: %s", screenDrawCorners) + logger.debug("Test point %s", a) + logger.debug("Transformed point %s", transform(a[0:2])) # exit() else: coordinates = {'tl': None, 'tr': None, 'bl': None, 'br': None} - transformationMatrix = None + transform = None + +windowRoot = Tkinter.Tk() +windowSize = (1000,1000) +windowRoot.geometry('%dx%d+%d+%d' % (windowSize[0],windowSize[1],0,0)) +canvas = Tkinter.Canvas(windowRoot,width=1000,height=1000) +canvas.pack() while True: _, im = c.read() @@ -167,7 +161,7 @@ while True: # will make everything bigger and allow us to detect more faces. dets = detector(im, 1) - print("Number of faces detected: {}".format(len(dets))) + logger.debug("Number of faces detected: {}".format(len(dets))) # We use this later for calibrating currentPoint = None @@ -178,7 +172,6 @@ while True: 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 @@ -218,7 +211,7 @@ while True: print("Error determening PnP", success) continue - print ("Rotation Vector:\n {0}".format(rotation_vector)) + logger.debug ("Rotation Vector:\n %s", rotation_vector) print ("Translation Vector:\n {0}".format(translation_vector)) # Project a 3D point (0, 0, 1000.0) onto the image plane. @@ -286,6 +279,7 @@ while True: 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: @@ -299,12 +293,13 @@ while True: # screen is 16:10 cv2.rectangle(im, (9, 59), (91, 111), (255,255,255), 1) - if transformationMatrix is None: + if transform 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: + newMetrics = np.zeros(metricsSize) for point in currentPoints: # check if within coordinates: # dot1 = np.dot(coordinates['tl'] - point, coordinates['tl'] - coordinates['br']) @@ -315,53 +310,76 @@ while True: 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) + # from 1920x1080 to 80x50 + miniTargetPoint = (int(targetPoint[0] / 1920 * 80 + 10), int(targetPoint[1] / 1080 * 50 + 60)) + cv2.circle(im, miniTargetPoint, 2, (0,255,0), -1) + targetInt = (int(targetPoint[0]), int(targetPoint[1])) + # check if point fits on screen: + # if so, measure it + if targetInt[0] >= 0 and targetInt[1] >= 0 and targetInt[0] < metrics.shape[0] and targetInt[1] < metrics.shape[1]: + newMetrics[targetInt[0],targetInt[1]] += 1 + # after we collected all new metrics, blur them foor smoothness + # and add to all metrics collected + metrics = metrics + gaussian_filter(newMetrics, sigma = 8) - # Display image + # Display webcam image with overlays cv2.imshow("Output", im) + logger.debug("showed webcam image") + + # blur smooth the heatmap + logger.debug("Max blurred metrics: %f", np.max(metrics)) + + # update the heatmap output + normalisedMetrics = metrics / (np.max(metrics)/255) + logger.debug("Max normalised metrics: %f", np.max(normalisedMetrics)) + print(normalisedMetrics) + image = Image.fromarray(normalisedMetrics) + wpercent = (windowSize[0] / float(image.size[0])) + hsize = int((float(image.size[1]) * float(wpercent))) + image = image.resize((windowSize[0], hsize)) + + tkpi = ImageTk.PhotoImage(image) + canvas.delete("IMG") + imagesprite = canvas.create_image(500,500,image=tkpi, tags="IMG") + windowRoot.update() + logger.debug("updated window") + + # (optionally) very slowly fade out previous metrics: + # metrics = metrics * .999 + keyPress = cv2.waitKey(5) if keyPress==27: break + elif keyPress == ord('d'): + logger.setLevel(logging.DEBUG) elif keyPress > -1 and currentPoint is not None: + recalculate = False if keyPress == ord('1'): coordinates['tl'] = currentPoint + recalculate = True elif keyPress == ord('2'): coordinates['tr'] = currentPoint + recalculate = True elif keyPress == ord('3'): coordinates['bl'] = currentPoint + recalculate = True 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 + recalculate = True + elif keyPress == ord('t') and transform is not None: + print("Coordinates", coordinates) + print("Drawing area", screenDrawCorners) + print("Test point %s", currentPoint ) + print("Transformed point %s", transform(currentPoint)) - # 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) + if recalculate is True and not any (x is None for x in coordinates.values()): + logger.debug(coordinates.values()) + pickle.dump( coordinates, open( "coordinates.p", "wb" ) ) + logger.info("Saved coordinates") - # 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)) + transform = create_perspective_transform(coordinatesToSrc(coordinates), screenDrawCorners) diff --git a/helpers.py b/helpers.py new file mode 100644 index 0000000..45cc724 --- /dev/null +++ b/helpers.py @@ -0,0 +1,101 @@ +import numpy as np + +def coordinatesToSrc(coordinates): + return np.array([coordinates['tl'], coordinates['tr'],coordinates['bl'], coordinates['br']]) + +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)) + return m + +# got this amazing thing from here: https://stackoverflow.com/a/24088499 +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 diff --git a/test.py b/test.py new file mode 100644 index 0000000..11327d4 --- /dev/null +++ b/test.py @@ -0,0 +1,20 @@ +import helpers +import numpy as np +import pickle + +screenDrawCorners = np.array([[10,60], [90, 60], [10, 110], [90, 110]]) +coordinates = pickle.load(open("coordinates.p", "rb")) +print("Loaded coordinates: %s", helpers.coordinatesToSrc(coordinates)) +print("Corners: %s", screenDrawCorners) + +transform = helpers.create_perspective_transform(helpers.coordinatesToSrc(coordinates), screenDrawCorners) + +a = [np.array([ 1312.15541183]), np.array([ 244.56278002])] +midpointTop = (coordinates['tr'] - coordinates['tl'])/2 + coordinates['tl'] +midpointCenter = (coordinates['tr'] - coordinates['bl'])/2 + coordinates['bl'] +print("Test point %s", a) +print("Transformed point %s", transform(a)) +print("Test point %s", midpointTop ) +print("Transformed point %s", transform(midpointTop)) +print("Test point %s", midpointCenter ) +print("Transformed point %s", transform(midpointCenter)) \ No newline at end of file