From 76bd3dcf3aebb9a93713890aeccf36210aa5cc6f Mon Sep 17 00:00:00 2001
From: Emiliana <38952746+emilianavt@users.noreply.github.com>
Date: Sun, 9 Aug 2020 19:34:14 +0200
Subject: [PATCH] Optimized estimate_depth function for a 0.5ms speedup.

---
 tracker.py | 36 +++++++++++++++++++++---------------
 1 file changed, 21 insertions(+), 15 deletions(-)

diff --git a/tracker.py b/tracker.py
index a9663c7..78c1495 100644
--- a/tracker.py
+++ b/tracker.py
@@ -708,7 +708,17 @@ class Tracker():
 
         rmat, _ = cv2.Rodrigues(rotation)
         inverse_rotation = np.linalg.inv(rmat)
-        pnp_error = 0.0
+        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())
+        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()
         for i, pt in enumerate(face_info.face_3d):
             if i == 68:
                 # Right eyeball
@@ -737,20 +747,16 @@ class Tracker():
                 d2 = np.linalg.norm(lms[i,0:2] - lms[45,0:2])
                 d = d1 + d2
                 pt = (pts_3d[42] * d1 + pts_3d[45] * d2) / d
-            reference = rmat.dot(pt)
-            reference = reference + face_info.translation
-            reference = self.camera.dot(reference)
-            depth = reference[2]
-            if i < 17 or i == 30:
-                reference = reference / depth
-                e1 = lms[i][0] - reference[0]
-                e2 = lms[i][1] - reference[1]
-                pnp_error += e1*e1 + e2*e2
-            pt_3d = np.array([lms[i][0] * depth, lms[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[i,:] = pt_3d[:]
+            if i == 66 or i == 67:
+                reference = rmat.dot(pt)
+                reference = reference + face_info.translation
+                reference = self.camera.dot(reference)
+                depth = reference[2]
+                pt_3d = np.array([lms[i][0] * depth, lms[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[i,:] = pt_3d[:]
 
         pnp_error = np.sqrt(pnp_error / (2.0 * image_pts.shape[0]))
         if pnp_error > 300: