Some changes

head_pose.py

@@ -471,15 +471,22 @@ p.daemon = True
 p.start()
 processes.append(p)
 
+newMetrics = np.zeros((metricsSize[1], metricsSize[0]))
+lastRunTime = 0
 
 while True:
+    result = None
+    try:
         te1 = time.time()
         result = pointsQueue.get()
         te1b = time.time()
         im = result['im']
         currentPoint = result['currentPoint']
         currentPoints = result['currentPoints']
+    except queue.Empty as e:
+        logger.warn('Result queue empty')
 
+    if result is not None:
         if not args.hide_preview:
             # draw little floorplan for 10 -> 50, sideplan 60 -> 100 (40x40 px)
             cv2.rectangle(im, (9, 9), (51, 51), (255,255,255), 1)
@@ -501,9 +508,6 @@ while True:
             tm3 = 0
             tm4 = 0
         else:
-        tm1 = time.time()
-        newMetrics = np.zeros((metricsSize[1], metricsSize[0]))
-        tm2 = time.time()
             for point in currentPoints:
                 # check if within coordinates:
                 # dot1 = np.dot(coordinates['tl'] - point, coordinates['tl'] - coordinates['br'])
@@ -539,24 +543,36 @@ while True:
             # and add to all metrics collected
             tm3 = time.time()
             # metrics = metrics + gaussian_filter(newMetrics, sigma = 13)
-        metrics = metrics + newMetrics
+
             tm4 = time.time()
-        logger.debug("Updated matrix with blur in %f", tm4 - tm3 + tm2 - tm1)
+            # logger.debug("Updated matrix with blur in %f", tm4 - tm3 + tm2 - tm1)
 
     # Display webcam image with overlays
     te2 = time.time()
-    if not args.hide_preview:
+    if result is not None and not args.hide_preview:
         cv2.imshow("Output", im)
     te3 = time.time()
     logger.debug("showed webcam image in %fs", te3-te2)
     logger.debug("Rendering took %fs", te3-te1)
     logger.debug("Waited took %fs", te1b-te1)
 
+
+
     # blur smooth the heatmap
     # logger.debug("Max blurred metrics: %f", np.max(metrics))
 
     # update the heatmap output
     tm21 = time.time()
+    t = tm21
+
+
+    diffT = min(1, t - lastRunTime)
+    # animDuration = 1
+    # factor = animDuration
+
+    metrics = metrics + newMetrics*diffT
+    newMetrics *= (1-diffT)
+
     # smooth impact of first hits by having at least 0.05
     normalisedMetrics = metrics / (max(255*7 ,np.max(metrics)))
     # convert to colormap, thanks to: https://stackoverflow.com/a/10967471