From 6002aeddde8261c50a488076c183011762a0e82c Mon Sep 17 00:00:00 2001
From: Daniel <daniel@mail.de>
Date: Mon, 5 Aug 2024 16:28:25 +0200
Subject: [PATCH] Small speed improvements.

---
 media/RESULTS.md             | 34 +++++++++++++++++-----------------
 scripts/triangulate_poses.py | 22 ++++++++++++++++------
 2 files changed, 33 insertions(+), 23 deletions(-)

diff --git a/media/RESULTS.md b/media/RESULTS.md
index 673b0df..b88d55a 100644
--- a/media/RESULTS.md
+++ b/media/RESULTS.md
@@ -7,9 +7,9 @@ Results of the model in various experiments on different datasets.
 (duration 00:01:20)
 ```json
 {
-  "avg_time_2d": 0.07095112719778288,
-  "avg_time_3d": 0.0030404001979504602,
-  "avg_fps": 13.515060915713226
+  "avg_time_2d": 0.0706314975932493,
+  "avg_time_3d": 0.002891659332534014,
+  "avg_fps": 13.6011569934277
 }
 {
   "person_nums": {
@@ -278,9 +278,9 @@ Results of the model in various experiments on different datasets.
 (duration 00:00:56)
 ```json
 {
-  "avg_time_2d": 0.10204474868643325,
-  "avg_time_3d": 0.017180594382007506,
-  "avg_fps": 8.38747848623052
+  "avg_time_2d": 0.10373067446181045,
+  "avg_time_3d": 0.01558385436067876,
+  "avg_fps": 8.381208976551004
 }
 {
   "person_nums": {
@@ -549,9 +549,9 @@ Results of the model in various experiments on different datasets.
 (duration 00:00:29)
 ```json
 {
-  "avg_time_2d": 0.05710376546068011,
-  "avg_time_3d": 0.005011487681910677,
-  "avg_fps": 16.09910528263349
+  "avg_time_2d": 0.059855266562047996,
+  "avg_time_3d": 0.004884123802185059,
+  "avg_fps": 15.446546443731663
 }
 {
   "person_nums": {
@@ -820,9 +820,9 @@ Results of the model in various experiments on different datasets.
 (duration 00:02:31)
 ```json
 {
-  "avg_time_2d": 0.05584677240189905,
-  "avg_time_3d": 0.0030521013817609993,
-  "avg_fps": 16.978253330837436
+  "avg_time_2d": 0.05529122129203044,
+  "avg_time_3d": 0.0028780115247735685,
+  "avg_fps": 17.191218649029846
 }
 {
   "person_nums": {
@@ -858,8 +858,8 @@ Results of the model in various experiments on different datasets.
     "ap-0.05": 9.5e-05,
     "ap-0.1": 0.257803,
     "ap-0.15": 0.500043,
-    "ap-0.25": 0.610092,
-    "ap-0.5": 0.638299
+    "ap-0.25": 0.610091,
+    "ap-0.5": 0.638298
   },
   "head": {
     "count": 753,
@@ -1031,9 +1031,9 @@ Results of the model in various experiments on different datasets.
 (duration 00:02:28)
 ```json
 {
-  "avg_time_2d": 0.10889388700810874,
-  "avg_time_3d": 0.02615876488569306,
-  "avg_fps": 7.404519540914655
+  "avg_time_2d": 0.10983607013051103,
+  "avg_time_3d": 0.023344272520483995,
+  "avg_fps": 7.508615611693869
 }
 {
   "person_nums": {
diff --git a/scripts/triangulate_poses.py b/scripts/triangulate_poses.py
index 4e18ab7..8360e3d 100644
--- a/scripts/triangulate_poses.py
+++ b/scripts/triangulate_poses.py
@@ -46,7 +46,7 @@ def undistort_points(points: np.ndarray, caminfo: dict):
     points = cv2.undistortPoints(points, K, DC, P=newK)
     points = points.reshape(pshape)
 
-    return points
+    return points, caminfo
 
 
 # ==================================================================================================
@@ -104,9 +104,7 @@ def calc_pose_scored(pose1, pose2, cam1, cam2, roomparams):
     # Triangulate points
     points1 = pose1[mask, 0:2].T
     points2 = pose2[mask, 0:2].T
-    P1 = get_camera_P(cam1)
-    P2 = get_camera_P(cam2)
-    points3d = cv2.triangulatePoints(P1, P2, points1, points2)
+    points3d = cv2.triangulatePoints(cam1["P"], cam2["P"], points1, points2)
     points3d = (points3d / points3d[3, :])[0:3, :].T
     pose3d = np.zeros([len(pose1), 4])
     pose3d[mask] = np.concatenate([points3d, np.ones([points3d.shape[0], 1])], axis=-1)
@@ -295,14 +293,26 @@ def get_3d_pose(poses_2d, camparams, roomparams, joint_names_2d, min_score=0.95)
         camparams[i]["K"] = np.array(camparams[i]["K"])
         camparams[i]["R"] = np.array(camparams[i]["R"])
         camparams[i]["T"] = np.array(camparams[i]["T"])
-        camparams[i]["DC"] = np.array(camparams[i]["DC"][0:5])
+        camparams[i]["DC"] = np.array(camparams[i]["DC"])
 
     # Undistort 2D points
     for i in range(len(camparams)):
         poses = poses_2d[i]
         cam = camparams[i]
-        poses[:, :, 0:2] = undistort_points(poses[:, :, 0:2], cam)
+        poses[:, :, 0:2], cam = undistort_points(poses[:, :, 0:2], cam)
+        # Mask out points that are far outside the image (points slightly outside are still valid)
+        offset = (cam["width"] + cam["height"]) / 40
+        mask = (
+            (poses[:, :, 0] >= 0 - offset)
+            & (poses[:, :, 0] < cam["width"] + offset)
+            & (poses[:, :, 1] >= 0 - offset)
+            & (poses[:, :, 1] < cam["height"] + offset)
+        )
+        poses = poses * np.expand_dims(mask, axis=-1)
         poses_2d[i] = poses
+        # Calc projection matrix with updated camera parameters
+        cam["P"] = get_camera_P(cam)
+        camparams[i] = cam
 
     # Create pairs of persons
     num_persons = [len(p) for p in poses_2d]