Draw out position in little floorplan

head_pose.py

@@ -4,6 +4,8 @@ import cv2
 import dlib
 import numpy as np
 
+from PIL import Image, ImageDraw
+ 
 # Read Image
 c = cv2.VideoCapture(0)
 # im = cv2.imread("headPose.jpg");
@@ -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,9 +86,65 @@ 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: