Rudimentary visualisation
This commit is contained in:
Ruben
parent
9be539e3a4
commit
1b049b9e5a
146
head_pose.py
146
head_pose.py
|
|
@ -5,9 +5,14 @@ import dlib
|
|||
import numpy as np
|
||||
import os
|
||||
import pickle
|
||||
|
||||
from PIL import Image, ImageDraw
|
||||
import logging
|
||||
from scipy.ndimage.filters import gaussian_filter
|
||||
import Tkinter
|
||||
from PIL import Image, ImageDraw,ImageTk
|
||||
|
||||
logging.basicConfig( format='%(asctime)-15s %(name)s %(levelname)s: %(message)s' )
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
# Read Image
|
||||
c = cv2.VideoCapture(0)
|
||||
# im = cv2.imread("headPose.jpg");
|
||||
|
|
@ -20,6 +25,10 @@ predictor = dlib.shape_predictor(predictor_path)
|
|||
|
||||
screenDrawCorners = np.array([[10,60], [90, 60], [10, 110], [90, 110]])
|
||||
|
||||
# metrics matrix
|
||||
metricsSize = [1920,1080]
|
||||
metrics = np.zeros(metricsSize)
|
||||
screenDrawCorners = np.array([[0,0], [1919,0], [0, 1079], [1919,1079]])
|
||||
|
||||
def create_perspective_transform_matrix(src, dst):
|
||||
""" Creates a perspective transformation matrix which transforms points
|
||||
|
|
@ -42,24 +51,10 @@ def create_perspective_transform_matrix(src, dst):
|
|||
B = np.array(dst).reshape(8)
|
||||
af = np.dot(np.linalg.inv(A.T * A) * A.T, B)
|
||||
m = np.append(np.array(af).reshape(8), 1).reshape((3, 3))
|
||||
print("Created transformmatrix:", src, dst, m)
|
||||
logger.info("Created transformmatrix: src %s dst %s m %s", src, dst, m)
|
||||
return m
|
||||
|
||||
def transMatrix2(fromMatrix, toMatrix):
|
||||
matrix = []
|
||||
for p1, p2 in zip(toMatrix, fromMatrix):
|
||||
matrix.append([p1[0], p1[1], 1, 0, 0, 0, -p2[0]*p1[0], -p2[0]*p1[1]])
|
||||
matrix.append([0, 0, 0, p1[0], p1[1], 1, -p2[1]*p1[0], -p2[1]*p1[1]])
|
||||
|
||||
A = np.matrix(matrix, dtype=np.float)
|
||||
B = np.array(fromMatrix).reshape(8)
|
||||
|
||||
res = np.dot(np.linalg.inv(A.T * A) * A.T, B)
|
||||
m = np.append(np.array(res).reshape(8), 1).reshape((3,3))
|
||||
return m
|
||||
|
||||
|
||||
|
||||
# got this amazing thing from here: https://stackoverflow.com/a/24088499
|
||||
def create_perspective_transform(src, dst, round=False, splat_args=False):
|
||||
""" Returns a function which will transform points in quadrilateral
|
||||
``src`` to the corresponding points on quadrilateral ``dst``::
|
||||
|
|
@ -139,24 +134,23 @@ def coordinatesToSrc(coordinates):
|
|||
# coordinates of the screen boundaries
|
||||
if os.path.exists("coordinates.p"):
|
||||
coordinates = pickle.load(open("coordinates.p", "rb"))
|
||||
transformationMatrix = create_perspective_transform_matrix(coordinatesToSrc(coordinates), screenDrawCorners)
|
||||
transformationMatrix2 = transMatrix2(coordinatesToSrc(coordinates), screenDrawCorners)
|
||||
transform = create_perspective_transform(coordinatesToSrc(coordinates), screenDrawCorners)
|
||||
|
||||
a = [np.array([ 1312.15541183]), np.array([ 244.56278002]), 0]
|
||||
# a = [np.array([ 100.15541183]), np.array([ 244.56278002]), 0]
|
||||
print("Coords", coordinatesToSrc(coordinates))
|
||||
print("Corners:", screenDrawCorners)
|
||||
print("src", a)
|
||||
print("C", transformationMatrix)
|
||||
print("C2", transformationMatrix2)
|
||||
print("new point", np.dot(a, transformationMatrix))
|
||||
print("new point 2", np.dot(a, transformationMatrix2))
|
||||
print("new point 2", np.dot(transformationMatrix2, a))
|
||||
print("new point 3", transform(a[0:2]))
|
||||
logger.info("Loaded coordinates: %s", coordinatesToSrc(coordinates))
|
||||
logger.debug("Corners: %s", screenDrawCorners)
|
||||
logger.debug("Test point %s", a)
|
||||
logger.debug("Transformed point %s", transform(a[0:2]))
|
||||
# exit()
|
||||
else:
|
||||
coordinates = {'tl': None, 'tr': None, 'bl': None, 'br': None}
|
||||
transformationMatrix = None
|
||||
transform = None
|
||||
|
||||
windowRoot = Tkinter.Tk()
|
||||
windowSize = (1000,1000)
|
||||
windowRoot.geometry('%dx%d+%d+%d' % (windowSize[0],windowSize[1],0,0))
|
||||
canvas = Tkinter.Canvas(windowRoot,width=1000,height=1000)
|
||||
canvas.pack()
|
||||
|
||||
while True:
|
||||
_, im = c.read()
|
||||
|
|
@ -167,7 +161,7 @@ while True:
|
|||
# will make everything bigger and allow us to detect more faces.
|
||||
dets = detector(im, 1)
|
||||
|
||||
print("Number of faces detected: {}".format(len(dets)))
|
||||
logger.debug("Number of faces detected: {}".format(len(dets)))
|
||||
|
||||
# We use this later for calibrating
|
||||
currentPoint = None
|
||||
|
|
@ -178,7 +172,6 @@ while True:
|
|||
for d in dets:
|
||||
shape = predictor(im, d)
|
||||
|
||||
print(shape.part(30).x, shape.part(54))
|
||||
#2D image points. If you change the image, you need to change vector
|
||||
image_points = np.array([
|
||||
(shape.part(30).x,shape.part(30).y), # Nose tip
|
||||
|
|
@ -218,7 +211,7 @@ while True:
|
|||
print("Error determening PnP", success)
|
||||
continue
|
||||
|
||||
print ("Rotation Vector:\n {0}".format(rotation_vector))
|
||||
logger.debug ("Rotation Vector:\n %s", rotation_vector)
|
||||
print ("Translation Vector:\n {0}".format(translation_vector))
|
||||
|
||||
# Project a 3D point (0, 0, 1000.0) onto the image plane.
|
||||
|
|
@ -286,6 +279,7 @@ while True:
|
|||
|
||||
currentPoint = point
|
||||
currentPoints.append(point)
|
||||
|
||||
# TODO only draw nose line now, so we can change color depending whether on screen or not
|
||||
|
||||
# processed all faces, now draw on screen:
|
||||
|
|
@ -299,12 +293,13 @@ while True:
|
|||
# screen is 16:10
|
||||
cv2.rectangle(im, (9, 59), (91, 111), (255,255,255), 1)
|
||||
|
||||
if transformationMatrix is None:
|
||||
if transform is None:
|
||||
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))
|
||||
else:
|
||||
newMetrics = np.zeros(metricsSize)
|
||||
for point in currentPoints:
|
||||
# check if within coordinates:
|
||||
# dot1 = np.dot(coordinates['tl'] - point, coordinates['tl'] - coordinates['br'])
|
||||
|
|
@ -315,53 +310,76 @@ while True:
|
|||
targetPoint = transform(point)
|
||||
print("Looking at", point, targetPoint)
|
||||
# cv2.circle(im, (int(targetPoint[0]), int(targetPoint[1])), 2, (0,255,0), -1)
|
||||
cv2.circle(im, (int(targetPoint[0]), int(targetPoint[1])), 2, (0,255,0), -1)
|
||||
# from 1920x1080 to 80x50
|
||||
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] >= 0 and targetInt[1] >= 0 and targetInt[0] < metrics.shape[0] and targetInt[1] < metrics.shape[1]:
|
||||
newMetrics[targetInt[0],targetInt[1]] += 1
|
||||
# after we collected all new metrics, blur them foor smoothness
|
||||
# and add to all metrics collected
|
||||
metrics = metrics + gaussian_filter(newMetrics, sigma = 8)
|
||||
|
||||
# Display image
|
||||
# Display webcam image with overlays
|
||||
cv2.imshow("Output", im)
|
||||
logger.debug("showed webcam image")
|
||||
|
||||
# blur smooth the heatmap
|
||||
logger.debug("Max blurred metrics: %f", np.max(metrics))
|
||||
|
||||
# update the heatmap output
|
||||
normalisedMetrics = metrics / (np.max(metrics)/255)
|
||||
logger.debug("Max normalised metrics: %f", np.max(normalisedMetrics))
|
||||
print(normalisedMetrics)
|
||||
image = Image.fromarray(normalisedMetrics)
|
||||
wpercent = (windowSize[0] / float(image.size[0]))
|
||||
hsize = int((float(image.size[1]) * float(wpercent)))
|
||||
image = image.resize((windowSize[0], hsize))
|
||||
|
||||
tkpi = ImageTk.PhotoImage(image)
|
||||
canvas.delete("IMG")
|
||||
imagesprite = canvas.create_image(500,500,image=tkpi, tags="IMG")
|
||||
windowRoot.update()
|
||||
logger.debug("updated window")
|
||||
|
||||
# (optionally) very slowly fade out previous metrics:
|
||||
# metrics = metrics * .999
|
||||
|
||||
keyPress = cv2.waitKey(5)
|
||||
|
||||
if keyPress==27:
|
||||
break
|
||||
elif keyPress == ord('d'):
|
||||
logger.setLevel(logging.DEBUG)
|
||||
elif keyPress > -1 and currentPoint is not None:
|
||||
recalculate = False
|
||||
if keyPress == ord('1'):
|
||||
coordinates['tl'] = currentPoint
|
||||
recalculate = True
|
||||
elif keyPress == ord('2'):
|
||||
coordinates['tr'] = currentPoint
|
||||
recalculate = True
|
||||
elif keyPress == ord('3'):
|
||||
coordinates['bl'] = currentPoint
|
||||
recalculate = True
|
||||
elif keyPress == ord('4'):
|
||||
coordinates['br'] = currentPoint
|
||||
|
||||
print(coordinates.values())
|
||||
pickle.dump( coordinates, open( "coordinates.p", "wb" ) )
|
||||
print("Saved coordinates")
|
||||
if not any (x is None for x in coordinates.values()):
|
||||
# measured corners for corner pin
|
||||
# fromMatrix = np.array(coordinates.values())
|
||||
# # Drawing area:
|
||||
# toMatrix = screenDrawCorners
|
||||
recalculate = True
|
||||
elif keyPress == ord('t') and transform is not None:
|
||||
print("Coordinates", coordinates)
|
||||
print("Drawing area", screenDrawCorners)
|
||||
print("Test point %s", currentPoint )
|
||||
print("Transformed point %s", transform(currentPoint))
|
||||
|
||||
# matrix = []
|
||||
# for p1, p2 in zip(toMatrix, fromMatrix):
|
||||
# matrix.append([p1[0], p1[1], 1, 0, 0, 0, -p2[0]*p1[0], -p2[0]*p1[1]])
|
||||
# matrix.append([0, 0, 0, p1[0], p1[1], 1, -p2[1]*p1[0], -p2[1]*p1[1]])
|
||||
|
||||
# A = np.matrix(matrix, dtype=np.float)
|
||||
# B = np.array(fromMatrix).reshape(8)
|
||||
if recalculate is True and not any (x is None for x in coordinates.values()):
|
||||
logger.debug(coordinates.values())
|
||||
pickle.dump( coordinates, open( "coordinates.p", "wb" ) )
|
||||
logger.info("Saved coordinates")
|
||||
|
||||
# res = np.dot(np.linalg.inv(A.T * A) * A.T, B)
|
||||
# transformationMatrix = np.array(res).reshape(8)
|
||||
transformationMatrix = create_perspective_transform_matrix(coordinatesToSrc(coordinates), screenDrawCorners)
|
||||
|
||||
# measured corners for corner pin
|
||||
# fromMatrix = np.array(coordinates.values())
|
||||
# # Drawing area:
|
||||
# toMatrix = np.array([[10,60], [90, 60], [10, 110], [90, 110]])
|
||||
# print(fromMatrix, toMatrix)
|
||||
# print(np.linalg.inv(toMatrix))
|
||||
# # matrix to transform from measured to drawed space
|
||||
# transformationMatrix = np.dot(fromMatrix, np.linalg.inv(toMatrix))
|
||||
transform = create_perspective_transform(coordinatesToSrc(coordinates), screenDrawCorners)
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,101 @@
|
|||
import numpy as np
|
||||
|
||||
def coordinatesToSrc(coordinates):
|
||||
return np.array([coordinates['tl'], coordinates['tr'],coordinates['bl'], coordinates['br']])
|
||||
|
||||
def create_perspective_transform_matrix(src, dst):
|
||||
""" Creates a perspective transformation matrix which transforms points
|
||||
in quadrilateral ``src`` to the corresponding points on quadrilateral
|
||||
``dst``.
|
||||
|
||||
Will raise a ``np.linalg.LinAlgError`` on invalid input.
|
||||
"""
|
||||
# See:
|
||||
# * http://xenia.media.mit.edu/~cwren/interpolator/
|
||||
# * http://stackoverflow.com/a/14178717/71522
|
||||
in_matrix = []
|
||||
for (x, y), (X, Y) in zip(src, dst):
|
||||
in_matrix.extend([
|
||||
[x, y, 1, 0, 0, 0, -X * x, -X * y],
|
||||
[0, 0, 0, x, y, 1, -Y * x, -Y * y],
|
||||
])
|
||||
|
||||
A = np.matrix(in_matrix, dtype=np.float)
|
||||
B = np.array(dst).reshape(8)
|
||||
af = np.dot(np.linalg.inv(A.T * A) * A.T, B)
|
||||
m = np.append(np.array(af).reshape(8), 1).reshape((3, 3))
|
||||
return m
|
||||
|
||||
# got this amazing thing from here: https://stackoverflow.com/a/24088499
|
||||
def create_perspective_transform(src, dst, round=False, splat_args=False):
|
||||
""" Returns a function which will transform points in quadrilateral
|
||||
``src`` to the corresponding points on quadrilateral ``dst``::
|
||||
|
||||
>>> transform = create_perspective_transform(
|
||||
... [(0, 0), (10, 0), (10, 10), (0, 10)],
|
||||
... [(50, 50), (100, 50), (100, 100), (50, 100)],
|
||||
... )
|
||||
>>> transform((5, 5))
|
||||
(74.99999999999639, 74.999999999999957)
|
||||
|
||||
If ``round`` is ``True`` then points will be rounded to the nearest
|
||||
integer and integer values will be returned.
|
||||
|
||||
>>> transform = create_perspective_transform(
|
||||
... [(0, 0), (10, 0), (10, 10), (0, 10)],
|
||||
... [(50, 50), (100, 50), (100, 100), (50, 100)],
|
||||
... round=True,
|
||||
... )
|
||||
>>> transform((5, 5))
|
||||
(75, 75)
|
||||
|
||||
If ``splat_args`` is ``True`` the function will accept two arguments
|
||||
instead of a tuple.
|
||||
|
||||
>>> transform = create_perspective_transform(
|
||||
... [(0, 0), (10, 0), (10, 10), (0, 10)],
|
||||
... [(50, 50), (100, 50), (100, 100), (50, 100)],
|
||||
... splat_args=True,
|
||||
... )
|
||||
>>> transform(5, 5)
|
||||
(74.99999999999639, 74.999999999999957)
|
||||
|
||||
If the input values yield an invalid transformation matrix an identity
|
||||
function will be returned and the ``error`` attribute will be set to a
|
||||
description of the error::
|
||||
|
||||
>>> tranform = create_perspective_transform(
|
||||
... np.zeros((4, 2)),
|
||||
... np.zeros((4, 2)),
|
||||
... )
|
||||
>>> transform((5, 5))
|
||||
(5.0, 5.0)
|
||||
>>> transform.error
|
||||
'invalid input quads (...): Singular matrix
|
||||
"""
|
||||
try:
|
||||
transform_matrix = create_perspective_transform_matrix(src, dst)
|
||||
error = None
|
||||
except np.linalg.LinAlgError as e:
|
||||
transform_matrix = np.identity(3, dtype=np.float)
|
||||
error = "invalid input quads (%s and %s): %s" %(src, dst, e)
|
||||
error = error.replace("\n", "")
|
||||
|
||||
to_eval = "def perspective_transform(%s):\n" %(
|
||||
splat_args and "*pt" or "pt",
|
||||
)
|
||||
to_eval += " res = np.dot(transform_matrix, ((pt[0], ), (pt[1], ), (1, )))\n"
|
||||
to_eval += " res = res / res[2]\n"
|
||||
if round:
|
||||
to_eval += " return (int(round(res[0][0])), int(round(res[1][0])))\n"
|
||||
else:
|
||||
to_eval += " return (res[0][0], res[1][0])\n"
|
||||
locals = {
|
||||
"transform_matrix": transform_matrix,
|
||||
}
|
||||
locals.update(globals())
|
||||
exec to_eval in locals, locals
|
||||
res = locals["perspective_transform"]
|
||||
res.matrix = transform_matrix
|
||||
res.error = error
|
||||
return res
|
||||
|
|
@ -0,0 +1,20 @@
|
|||
import helpers
|
||||
import numpy as np
|
||||
import pickle
|
||||
|
||||
screenDrawCorners = np.array([[10,60], [90, 60], [10, 110], [90, 110]])
|
||||
coordinates = pickle.load(open("coordinates.p", "rb"))
|
||||
print("Loaded coordinates: %s", helpers.coordinatesToSrc(coordinates))
|
||||
print("Corners: %s", screenDrawCorners)
|
||||
|
||||
transform = helpers.create_perspective_transform(helpers.coordinatesToSrc(coordinates), screenDrawCorners)
|
||||
|
||||
a = [np.array([ 1312.15541183]), np.array([ 244.56278002])]
|
||||
midpointTop = (coordinates['tr'] - coordinates['tl'])/2 + coordinates['tl']
|
||||
midpointCenter = (coordinates['tr'] - coordinates['bl'])/2 + coordinates['bl']
|
||||
print("Test point %s", a)
|
||||
print("Transformed point %s", transform(a))
|
||||
print("Test point %s", midpointTop )
|
||||
print("Transformed point %s", transform(midpointTop))
|
||||
print("Test point %s", midpointCenter )
|
||||
print("Transformed point %s", transform(midpointCenter))
|
||||
Loading…
Reference in New Issue