From ca87917297fd53205d93b448318d5653f7354cad Mon Sep 17 00:00:00 2001 From: Ruben Date: Tue, 24 Apr 2018 12:40:33 +0200 Subject: [PATCH] Draw out position in little floorplan --- head_pose.py | 76 +++++++++++++++++++++++++++++++++++++++++++++------- 1 file changed, 67 insertions(+), 9 deletions(-) diff --git a/head_pose.py b/head_pose.py index 4528781..85c3c69 100644 --- a/head_pose.py +++ b/head_pose.py @@ -3,6 +3,8 @@ import cv2 import dlib import numpy as np + +from PIL import Image, ImageDraw # Read Image c = cv2.VideoCapture(0) @@ -54,7 +56,6 @@ while True: # Camera internals - focal_length = size[1] center = (size[1]/2, size[0]/2) camera_matrix = np.array( @@ -63,19 +64,20 @@ while True: [0, 0, 1]], dtype = "double" ) - print ("Camera Matrix :\n {0}".format(camera_matrix)) + # 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: @@ -84,12 +86,68 @@ while True: 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) + + # 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 + + 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? + + # rotatedVector = np.dot(rotMatrix, translation_vector) + # print("rvec", rotatedVector) + + # 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) + + + cv2.line(im, (mapPosZ + 10, mapPosX + 10), (mapPosZ + 10 + int(rotation_vector[2]*5), mapPosX + 10 + int(rotation_vector[0]*5)), (255,255,0), 1) + cv2.line(im, (mapPosZ + 60, mapPosY + 10), (mapPosZ + 60 + int(rotation_vector[2]*5), mapPosY + 10 + int(rotation_vector[1]*50)), (255,255,0), 1) + # print (a, x, y, rotMatrix) + # 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) + # Display image cv2.imshow("Output", im) if cv2.waitKey(5)==27: break -cv2.destroyAllWindows() \ No newline at end of file +cv2.destroyAllWindows()