diff --git a/face.py b/face.py index 52152b4..ffafc8c 100644 --- a/face.py +++ b/face.py @@ -86,6 +86,7 @@ class FaceInfo(): self.coord = None self.base_scale_v = self.face_3d[27:30, 1] - self.face_3d[28:31, 1] self.base_scale_h = np.abs(self.face_3d[[0, 36, 42], 0] - self.face_3d[[16, 39, 45], 0]) + self.fail_count = 0 def reset(self): self.alive = False diff --git a/landmarks.py b/landmarks.py index aa6fad1..882fe20 100644 --- a/landmarks.py +++ b/landmarks.py @@ -50,117 +50,101 @@ def run( models, crop, crop_info): return (confidence, lms) -#I haven't touched much here -#not because I didn't try to improve things -#but because I caused problems when I did -class Landmarks(): - def __init__(self, width, height, threshold): +def estimate_depth( face_info, width, height): + camera = np.array([[width, 0, width/2], [0, width, height/2], [0, 0, 1]], np.float32) + inverse_camera = np.linalg.inv(camera) + contour_pts = [0,1,8,15,16,27,28,29,30,31,32,33,34,35] + lms = np.concatenate((face_info.lms, np.array([[face_info.eye_state[0][1], face_info.eye_state[0][2], face_info.eye_state[0][3]], [face_info.eye_state[1][1], face_info.eye_state[1][2], face_info.eye_state[1][3]]], np.float32)), 0) + image_pts = np.array(lms)[contour_pts, 0:2] - self.camera = np.array([[width, 0, width/2], [0, width, height/2], [0, 0, 1]], np.float32) - self.inverse_camera = np.linalg.inv(self.camera) - self.width = width - self.height = height - self.fail_count = 0 + success = False + if face_info.rotation is not None: + success, face_info.rotation, face_info.translation = cv2.solvePnP(face_info.contour, image_pts, camera, np.zeros((4,1)), useExtrinsicGuess=True, rvec=np.transpose(face_info.rotation), tvec=np.transpose(face_info.translation), flags=cv2.SOLVEPNP_ITERATIVE) + else: + rvec = np.array([0, 0, 0], np.float32) + tvec = np.array([0, 0, 0], np.float32) + success, face_info.rotation, face_info.translation = cv2.solvePnP(face_info.contour, image_pts, camera, np.zeros((4,1)), useExtrinsicGuess=True, rvec=rvec, tvec=tvec, flags=cv2.SOLVEPNP_ITERATIVE) + if not success: + face_info.rotation = np.array([0.0, 0.0, 0.0], np.float32) + face_info.translation = np.array([0.0, 0.0, 0.0], np.float32) + return False, np.zeros(4), np.zeros(3), 99999., pts_3d, lms + rotation = face_info.rotation + translation = face_info.translation + pts_3d = np.zeros((70,3), np.float32) + face_info.rotation = np.transpose(face_info.rotation) + face_info.translation = np.transpose(face_info.translation) + rmat, _ = cv2.Rodrigues(rotation) + inverse_rotation = np.linalg.inv(rmat) + t_reference = face_info.face_3d.dot(rmat.transpose()) + t_reference = t_reference + face_info.translation + t_reference = t_reference.dot(camera.transpose()) + t_depth = t_reference[:, 2] + t_depth[t_depth == 0] = 0.000001 + t_depth_e = np.expand_dims(t_depth[:],1) + t_reference = t_reference[:] / t_depth_e + pts_3d[0:66] = np.stack([lms[0:66,0], lms[0:66,1], np.ones((66,))], 1) * t_depth_e[0:66] + pts_3d[0:66] = (pts_3d[0:66].dot(inverse_camera.transpose()) - face_info.translation).dot(inverse_rotation.transpose()) + for i, pt in enumerate(face_info.face_3d[66:70]): + if i == 2: + # Right eyeball + # Eyeballs have an average diameter of 12.5mm and and the distance between eye corners is 30-35mm, so a conversion factor of 0.385 can be applied + eye_center = (pts_3d[36] + pts_3d[39]) / 2.0 + d_corner = np.linalg.norm(pts_3d[36] - pts_3d[39]) + depth = 0.385 * d_corner + pt_3d = np.array([eye_center[0], eye_center[1], eye_center[2] - depth]) + pts_3d[68] = pt_3d + continue + if i == 3: + # Left eyeball + eye_center = (pts_3d[42] + pts_3d[45]) / 2.0 + d_corner = np.linalg.norm(pts_3d[42] - pts_3d[45]) + depth = 0.385 * d_corner + pt_3d = np.array([eye_center[0], eye_center[1], eye_center[2] - depth]) + pts_3d[69] = pt_3d + continue + if i == 0: + d1 = np.linalg.norm(lms[66,0:2] - lms[36,0:2]) + d2 = np.linalg.norm(lms[66,0:2] - lms[39,0:2]) + d = d1 + d2 + pt = (pts_3d[36] * d1 + pts_3d[39] * d2) / d + if i == 1: + d1 = np.linalg.norm(lms[67,0:2] - lms[42,0:2]) + d2 = np.linalg.norm(lms[67,0:2] - lms[45,0:2]) + d = d1 + d2 + pt = (pts_3d[42] * d1 + pts_3d[45] * d2) / d + if i < 2: + reference = rmat.dot(pt) + reference = reference + face_info.translation + reference = camera.dot(reference) + depth = reference[2] + pt_3d = np.array([lms[66+i][0] * depth, lms[66+i][1] * depth, depth], np.float32) + pt_3d = inverse_camera.dot(pt_3d) + pt_3d = pt_3d - face_info.translation + pt_3d = inverse_rotation.dot(pt_3d) + pts_3d[66+i,:] = pt_3d[:] - #I really need to break this guy up into smaller parts - def estimate_depth(self, face_info): - contour_pts = [0,1,8,15,16,27,28,29,30,31,32,33,34,35] - lms = np.concatenate((face_info.lms, np.array([[face_info.eye_state[0][1], face_info.eye_state[0][2], face_info.eye_state[0][3]], [face_info.eye_state[1][1], face_info.eye_state[1][2], face_info.eye_state[1][3]]], np.float32)), 0) - image_pts = np.array(lms)[contour_pts, 0:2] - - success = False - if face_info.rotation is not None: - success, face_info.rotation, face_info.translation = cv2.solvePnP(face_info.contour, image_pts, self.camera, np.zeros((4,1)), useExtrinsicGuess=True, rvec=np.transpose(face_info.rotation), tvec=np.transpose(face_info.translation), flags=cv2.SOLVEPNP_ITERATIVE) - else: - rvec = np.array([0, 0, 0], np.float32) - tvec = np.array([0, 0, 0], np.float32) - success, face_info.rotation, face_info.translation = cv2.solvePnP(face_info.contour, image_pts, self.camera, np.zeros((4,1)), useExtrinsicGuess=True, rvec=rvec, tvec=tvec, flags=cv2.SOLVEPNP_ITERATIVE) - - if not success: - face_info.rotation = np.array([0.0, 0.0, 0.0], np.float32) + pts_3d[np.isnan(pts_3d).any(axis=1)] = np.array([0.,0.,0.], dtype=np.float32) + pnp_error = np.power(lms[0:17,0:2] - t_reference[0:17,0:2], 2).sum() + pnp_error += np.power(lms[30,0:2] - t_reference[30,0:2], 2).sum() + if np.isnan(pnp_error): + pnp_error = 9999999. + pnp_error = math.sqrt(pnp_error / (2.0 * image_pts.shape[0])) + if pnp_error > 300: + face_info.fail_count += 1 + if face_info.fail_count > 5: + # Something went wrong with adjusting the 3D model + print(f"Detected anomaly when 3D fitting face {0}. Resetting.") + face_info.rotation = None face_info.translation = np.array([0.0, 0.0, 0.0], np.float32) - return False, np.zeros(4), np.zeros(3), 99999., pts_3d, lms + face_info.update_counts = np.zeros((66,2)) + face_info.update_contour() + else: + face_info.fail_count = 0 - rotation = face_info.rotation - translation = face_info.translation - pts_3d = np.zeros((70,3), np.float32) - - face_info.rotation = np.transpose(face_info.rotation) - face_info.translation = np.transpose(face_info.translation) - - rmat, _ = cv2.Rodrigues(rotation) - inverse_rotation = np.linalg.inv(rmat) - t_reference = face_info.face_3d.dot(rmat.transpose()) - t_reference = t_reference + face_info.translation - t_reference = t_reference.dot(self.camera.transpose()) - t_depth = t_reference[:, 2] - t_depth[t_depth == 0] = 0.000001 - t_depth_e = np.expand_dims(t_depth[:],1) - t_reference = t_reference[:] / t_depth_e - pts_3d[0:66] = np.stack([lms[0:66,0], lms[0:66,1], np.ones((66,))], 1) * t_depth_e[0:66] - pts_3d[0:66] = (pts_3d[0:66].dot(self.inverse_camera.transpose()) - face_info.translation).dot(inverse_rotation.transpose()) - - for i, pt in enumerate(face_info.face_3d[66:70]): - if i == 2: - # Right eyeball - # Eyeballs have an average diameter of 12.5mm and and the distance between eye corners is 30-35mm, so a conversion factor of 0.385 can be applied - eye_center = (pts_3d[36] + pts_3d[39]) / 2.0 - d_corner = np.linalg.norm(pts_3d[36] - pts_3d[39]) - depth = 0.385 * d_corner - pt_3d = np.array([eye_center[0], eye_center[1], eye_center[2] - depth]) - pts_3d[68] = pt_3d - continue - if i == 3: - # Left eyeball - eye_center = (pts_3d[42] + pts_3d[45]) / 2.0 - d_corner = np.linalg.norm(pts_3d[42] - pts_3d[45]) - depth = 0.385 * d_corner - pt_3d = np.array([eye_center[0], eye_center[1], eye_center[2] - depth]) - pts_3d[69] = pt_3d - continue - if i == 0: - d1 = np.linalg.norm(lms[66,0:2] - lms[36,0:2]) - d2 = np.linalg.norm(lms[66,0:2] - lms[39,0:2]) - d = d1 + d2 - pt = (pts_3d[36] * d1 + pts_3d[39] * d2) / d - if i == 1: - d1 = np.linalg.norm(lms[67,0:2] - lms[42,0:2]) - d2 = np.linalg.norm(lms[67,0:2] - lms[45,0:2]) - d = d1 + d2 - pt = (pts_3d[42] * d1 + pts_3d[45] * d2) / d - if i < 2: - reference = rmat.dot(pt) - reference = reference + face_info.translation - reference = self.camera.dot(reference) - depth = reference[2] - pt_3d = np.array([lms[66+i][0] * depth, lms[66+i][1] * depth, depth], np.float32) - pt_3d = self.inverse_camera.dot(pt_3d) - pt_3d = pt_3d - face_info.translation - pt_3d = inverse_rotation.dot(pt_3d) - pts_3d[66+i,:] = pt_3d[:] - - pts_3d[np.isnan(pts_3d).any(axis=1)] = np.array([0.,0.,0.], dtype=np.float32) - pnp_error = np.power(lms[0:17,0:2] - t_reference[0:17,0:2], 2).sum() - pnp_error += np.power(lms[30,0:2] - t_reference[30,0:2], 2).sum() - if np.isnan(pnp_error): - pnp_error = 9999999. - pnp_error = math.sqrt(pnp_error / (2.0 * image_pts.shape[0])) - if pnp_error > 300: - self.fail_count += 1 - if self.fail_count > 5: - # Something went wrong with adjusting the 3D model - print(f"Detected anomaly when 3D fitting face {0}. Resetting.") - face_info.rotation = None - face_info.translation = np.array([0.0, 0.0, 0.0], np.float32) - face_info.update_counts = np.zeros((66,2)) - face_info.update_contour() - else: - self.fail_count = 0 - - euler = cv2.RQDecomp3x3(rmat)[0] - return True, matrix_to_quaternion(rmat), euler, pnp_error, pts_3d, lms + euler = cv2.RQDecomp3x3(rmat)[0] + return True, matrix_to_quaternion(rmat), euler, pnp_error, pts_3d, lms diff --git a/tracker.py b/tracker.py index b1dff69..6a698b5 100644 --- a/tracker.py +++ b/tracker.py @@ -54,7 +54,6 @@ class Tracker(): self.detection_threshold = detection_threshold self.EyeTracker = eyes.EyeTracker() - self.Landmarks = landmarks.Landmarks(width, height, threshold) self.model = Models(model_type = model_type) # Image normalization constants @@ -144,7 +143,7 @@ class Tracker(): face_info = self.face_info if face_info.alive: - face_info.success, face_info.quaternion, face_info.euler, face_info.pnp_error, face_info.pts_3d, face_info.lms = self.Landmarks.estimate_depth(face_info) + face_info.success, face_info.quaternion, face_info.euler, face_info.pnp_error, face_info.pts_3d, face_info.lms = landmarks.estimate_depth(face_info, self.width, self.height) if face_info.success: face_info.adjust_3d()