diff --git a/media/RESULTS.md b/media/RESULTS.md
index dfa4eba..b506bf1 100644
--- a/media/RESULTS.md
+++ b/media/RESULTS.md
@@ -294,26 +294,26 @@ Results of the model in various experiments on different datasets. \
 
 ```json
 {
-  "img_loading": 0.0422137,
-  "demosaicing": 0.000738378,
-  "avg_time_2d": 0.0148837,
-  "avg_time_3d": 0.000220359,
-  "fps": 63.1215
+  "img_loading": 0.0424014,
+  "demosaicing": 0.000721194,
+  "avg_time_2d": 0.0148998,
+  "avg_time_3d": 0.000186437,
+  "fps": 63.2615
 }
 {
   "triangulator_calls": 301,
-  "init_time": 7.04818e-06,
-  "undistort_time": 3.18955e-05,
-  "project_time": 3.92248e-06,
-  "match_time": 1.17322e-05,
-  "pairs_time": 6.12719e-06,
-  "pair_scoring_time": 4.9701e-05,
-  "grouping_time": 8.03308e-06,
-  "full_time": 5.63869e-05,
-  "merge_time": 1.32789e-05,
-  "post_time": 9.21993e-06,
-  "convert_time": 2.10834e-07,
-  "total_time": 0.000197817
+  "init_time": 7.22995e-06,
+  "undistort_time": 2.88306e-05,
+  "project_time": 3.50066e-06,
+  "match_time": 1.16445e-05,
+  "pairs_time": 6.11458e-06,
+  "pair_scoring_time": 4.69781e-05,
+  "grouping_time": 7.95871e-06,
+  "full_time": 4.9667e-05,
+  "merge_time": 1.32834e-05,
+  "post_time": 9.24932e-06,
+  "convert_time": 2.06937e-07,
+  "total_time": 0.000184941
 }
 {
   "person_nums": {
diff --git a/rpt/triangulator.cpp b/rpt/triangulator.cpp
index b6babe4..cd6bca7 100644
--- a/rpt/triangulator.cpp
+++ b/rpt/triangulator.cpp
@@ -624,87 +624,61 @@ void TriangulatorInternal::undistort_poses(
             icam.K, icam.DC, cv::Size(width, height), 1, cv::Size(width, height));
     }
 
-    // Convert vectors to mats
+    // Convert vectors to single mat
     size_t num_persons = poses_2d.size();
     size_t num_joints = poses_2d[0].size();
-    std::vector<cv::Mat> poses;
-    for (size_t j = 0; j < num_persons; ++j)
+    std::vector<int> dims = {(int)(num_persons * num_joints), 2};
+    cv::Mat points_mat = cv::Mat(dims, CV_32F);
+    float *mat_ptr = points_mat.ptr<float>(0);
+    for (size_t i = 0; i < num_persons; ++i)
     {
-        std::vector<int> dims = {(int)num_joints, 3};
-        cv::Mat pose_mat = cv::Mat(dims, CV_32F);
-
-        // Use pointer to copy data efficiently
-        for (size_t k = 0; k < num_joints; ++k)
-        {
-            float *mat_ptr = pose_mat.ptr<float>(k);
-            const auto &joint = poses_2d[j][k];
-            mat_ptr[0] = joint[0];
-            mat_ptr[1] = joint[1];
-            mat_ptr[2] = joint[2];
-        }
-        poses.push_back(std::move(pose_mat));
-    }
-
-    for (size_t p = 0; p < poses.size(); ++p)
-    {
-        // Extract the (x, y) coordinates
-        cv::Mat points = poses[p].colRange(0, 2).clone();
-        points = points.reshape(2);
-
-        // Undistort the points
-        if (icam.cam.type == "fisheye")
-        {
-            cv::fisheye::undistortPoints(points, points, icam.K, icam.DC, cv::noArray(), newK);
-        }
-        else
-        {
-            cv::undistortPoints(points, points, icam.K, icam.DC, cv::noArray(), newK);
-        }
-
-        // Update the original poses with the undistorted points
-        points = points.reshape(1);
-        points.copyTo(poses[p].colRange(0, 2));
-
-        // Mask out points that are far outside the image (points slightly outside are still valid)
-        float mask_offset = (width + height) / 20.0;
-        int num_joints = poses[p].rows;
-        for (int j = 0; j < num_joints; ++j)
-        {
-            float *poses_ptr = poses[p].ptr<float>(j);
-            float x = poses_ptr[0];
-            float y = poses_ptr[1];
-            bool in_x = x >= -mask_offset && x < width + mask_offset;
-            bool in_y = y >= -mask_offset && y < height + mask_offset;
-            if (!in_x || !in_y)
-            {
-                poses_ptr[0] = 0.0;
-                poses_ptr[1] = 0.0;
-                poses_ptr[2] = 0.0;
-            }
-        }
-    }
-
-    // Convert mats to vectors
-    poses_2d.clear();
-    poses_2d.reserve(poses.size());
-    for (size_t i = 0; i < poses.size(); ++i)
-    {
-        std::vector<std::array<float, 3>> pose;
-        size_t num_joints = poses[i].rows;
-        pose.reserve(num_joints);
-
         for (size_t j = 0; j < num_joints; ++j)
         {
-            const float *mat_ptr = poses[i].ptr<float>(j);
-            std::array<float, 3> point;
-            for (size_t k = 0; k < 3; ++k)
+            for (size_t k = 0; k < 2; ++k)
             {
-                point[k] = mat_ptr[k];
+                mat_ptr[i * num_joints * 2 + j * 2 + k] = poses_2d[i][j][k];
             }
-            pose.push_back(point);
         }
+    }
 
-        poses_2d.push_back(std::move(pose));
+    // Undistort all the points
+    points_mat = points_mat.reshape(2, static_cast<int>(points_mat.rows));
+    if (icam.cam.type == "fisheye")
+    {
+        cv::fisheye::undistortPoints(points_mat, points_mat, icam.K, icam.DC, cv::noArray(), newK);
+    }
+    else
+    {
+        cv::undistortPoints(points_mat, points_mat, icam.K, icam.DC, cv::noArray(), newK);
+    }
+    points_mat = points_mat.reshape(1, static_cast<int>(points_mat.rows));
+
+    // Overwrite the old coordinates with the undistorted ones
+    for (size_t i = 0; i < num_persons; ++i)
+    {
+        for (size_t j = 0; j < num_joints; ++j)
+        {
+            for (size_t k = 0; k < 2; ++k)
+            {
+                poses_2d[i][j][k] = mat_ptr[i * num_joints * 2 + j * 2 + k];
+            }
+        }
+    }
+
+    // Mask out points that are far outside the image (points slightly outside are still valid)
+    float mask_offset = (width + height) / 20.0;
+    for (size_t i = 0; i < num_persons; ++i)
+    {
+        for (size_t j = 0; j < num_joints; ++j)
+        {
+            if (poses_2d[i][j][0] < -mask_offset || poses_2d[i][j][0] >= width + mask_offset ||
+                poses_2d[i][j][1] < -mask_offset || poses_2d[i][j][1] >= height + mask_offset)
+            {
+                poses_2d[i][j][0] = 0.0;
+                poses_2d[i][j][1] = 0.0;
+                poses_2d[i][j][2] = 0.0;
+            }
+        }
     }
 
     // Update the camera matrix