Rudimentary visualisation

head_pose.py

@@ -5,8 +5,13 @@ import dlib
 import numpy as np
 import os
 import pickle
+import logging
+from scipy.ndimage.filters import gaussian_filter
+import Tkinter
+from PIL import Image, ImageDraw,ImageTk
  
-from PIL import Image, ImageDraw
+logging.basicConfig( format='%(asctime)-15s %(name)s %(levelname)s: %(message)s' )
+logger = logging.getLogger(__name__)
 
 # Read Image
 c = cv2.VideoCapture(0)
@@ -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):
+# got this amazing thing from here: https://stackoverflow.com/a/24088499
-    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``::
@@ -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]
+    logger.info("Loaded coordinates: %s", coordinatesToSrc(coordinates))
-    print("Coords", coordinatesToSrc(coordinates))
+    logger.debug("Corners: %s", screenDrawCorners)
-    print("Corners:", screenDrawCorners)
+    logger.debug("Test point %s", a)
-    print("src", a)
+    logger.debug("Transformed point %s", transform(a[0:2]))
-    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
+    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
+            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))
             
-        print(coordinates.values())
+
+        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" ) )
-        print("Saved coordinates")
+            logger.info("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 = []
+            transform = create_perspective_transform(coordinatesToSrc(coordinates), screenDrawCorners)
-            # 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))
 
 
 

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

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))