r/sustaining_gazes_tnc
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

Seems to be able to finding coordinates on the screen

Browse source
This commit is contained in:
Ruben 2018-04-24 19:43:29 +02:00
parent ca87917297
commit d45b66304c
1 changed files with 34 additions and 18 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

52
head_pose.py
View file

@ -88,20 +88,6 @@ while True:
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)
@ -135,9 +121,36 @@ while True:
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)
# 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]*200)), (255,255,0), 1)
# print(rotMatrix)
viewDirectionVector = np.dot(np.array([0.0, 0.0, 1000.0]), rotMatrix)
cv2.line(im, (mapPosZ + 10, mapPosX + 10), (mapPosZ + 10 + int(viewDirectionVector[2] * 100), mapPosX + 10 + int(viewDirectionVector[0] * 100)), (255,255,0), 1)
cv2.line(im, (mapPosZ + 60, mapPosY + 10), (mapPosZ + 60 + int(viewDirectionVector[2] * 100), mapPosY + 10 - int(viewDirectionVector[1] * 100)), (255,0,255), 1)
# 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
a = - translation_vector[2] / viewDirectionVector[2]
x = translation_vector[0] + viewDirectionVector[0] * a
y = translation_vector[1] + viewDirectionVector[1] * a
print (a, x, y)
# draw little floorplan for 10 -> 50, sideplan 60 -> 100 (40x40 px)
cv2.rectangle(im, (9, 9), (51, 51), (255,255,255), 1)
@ -147,7 +160,10 @@ while True:
# Display image
cv2.imshow("Output", im)
if cv2.waitKey(5)==27:
keyPress = cv2.waitKey(5)
if keyPress==27:
break
cv2.destroyAllWindows()