diff --git a/head_pose.py b/head_pose.py index 743cbd5..6681f22 100644 --- a/head_pose.py +++ b/head_pose.py @@ -471,92 +471,108 @@ p.daemon = True p.start() processes.append(p) +newMetrics = np.zeros((metricsSize[1], metricsSize[0])) +lastRunTime = 0 while True: - te1 = time.time() - result = pointsQueue.get() - te1b = time.time() - im = result['im'] - currentPoint = result['currentPoint'] - currentPoints = result['currentPoints'] + 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 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) - 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 transform is None: + if result is not None: if not args.hide_preview: - 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)) - tm1 = 0 - tm2 = 0 - 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']) - # dot2 = np.dot(coordinates['bl'] - point, coordinates['tl'] - coordinates['br']) - # pointIn3 = [point[0], point[1], 0] - # targetPoint = np.dot(pointIn3, transformationMatrix) - # logger.info("Looking at", pointIn3, np.dot( transformationMatrix, pointIn3)) - targetPoint = transform(point) - logger.info("Looking at {} {}".format(point, targetPoint) ) - # cv2.circle(im, (int(targetPoint[0]), int(targetPoint[1])), 2, (0,255,0), -1) - # from 1920x1080 to 80x50 + # 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 transform is None: if not args.hide_preview: - 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]+spotSize[0] >= 0 and targetInt[1]+spotSize[1] >= 0 and targetInt[0]-spotSize[0] < metricsSize[0] and targetInt[1]-spotSize[0] < metricsSize[1]: - dataframe = dataframe.append({'x':targetInt[0],'y':targetInt[1]}, ignore_index=True) - logger.info("Put metric {},{} in metrix of {},{}".format(targetInt[1],targetInt[0], metricsSize[1], metricsSize[0])) - for sx in range(spotSize[0]): - for sy in range(spotSize[1]): - mx = targetInt[0] + sx - (spotSize[0]-1)/2 - my = targetInt[1] + sy - (spotSize[1]-1)/2 + 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)) + tm1 = 0 + tm2 = 0 + tm3 = 0 + tm4 = 0 + 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) + # logger.info("Looking at", pointIn3, np.dot( transformationMatrix, pointIn3)) + targetPoint = transform(point) + logger.info("Looking at {} {}".format(point, targetPoint) ) + # cv2.circle(im, (int(targetPoint[0]), int(targetPoint[1])), 2, (0,255,0), -1) + # from 1920x1080 to 80x50 + if not args.hide_preview: + 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]+spotSize[0] >= 0 and targetInt[1]+spotSize[1] >= 0 and targetInt[0]-spotSize[0] < metricsSize[0] and targetInt[1]-spotSize[0] < metricsSize[1]: + dataframe = dataframe.append({'x':targetInt[0],'y':targetInt[1]}, ignore_index=True) + logger.info("Put metric {},{} in metrix of {},{}".format(targetInt[1],targetInt[0], metricsSize[1], metricsSize[0])) + for sx in range(spotSize[0]): + for sy in range(spotSize[1]): + mx = targetInt[0] + sx - (spotSize[0]-1)/2 + my = targetInt[1] + sy - (spotSize[1]-1)/2 - if mx >= 0 and my >= 0 and mx < metricsSize[0] and my < metricsSize[1]: - newMetrics[my,mx] += spot[sx,sy] #/ 20 - # print("MAX",np.max(newMetrics)) + if mx >= 0 and my >= 0 and mx < metricsSize[0] and my < metricsSize[1]: + newMetrics[my,mx] += spot[sx,sy] #/ 20 + # print("MAX",np.max(newMetrics)) - # TODO: put in an image of a blurred spot & remove blur action + # TODO: put in an image of a blurred spot & remove blur action - # after we collected all new metrics, blur them foor smoothness - # 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) + # after we collected all new metrics, blur them foor smoothness + # and add to all metrics collected + tm3 = time.time() + # metrics = metrics + gaussian_filter(newMetrics, sigma = 13) + + tm4 = time.time() + # 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