Added filtering with last poses.

spt/triangulator.cpp

@@ -178,6 +178,74 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
         internal_cameras[i].update_projection_matrix();
     }
 
+    // Project last 3D poses to 2D
+    std::vector<std::tuple<std::vector<cv::Mat>, std::vector<cv::Mat>>> last_poses_2d;
+    if (!last_poses_3d.empty())
+    {
+        // Select core joints
+        std::vector<cv::Mat> last_core_poses;
+        for (size_t i = 0; i < last_poses_3d.size(); ++i)
+        {
+            cv::Mat pose = last_poses_3d[i];
+            std::vector<int> dims = {(int)core_joint_idx.size(), 4};
+            cv::Mat last_poses_core(dims, pose.type());
+
+            for (size_t j = 0; j < core_joint_idx.size(); ++j)
+            {
+                pose.row(core_joint_idx[j]).copyTo(last_poses_core.row(j));
+            }
+            last_core_poses.push_back(last_poses_core);
+        }
+
+        // Project
+        last_poses_2d.reserve(cameras.size());
+        for (size_t i = 0; i < cameras.size(); ++i)
+        {
+            auto [poses2d, dists] = project_poses(last_core_poses, internal_cameras[i], true);
+            last_poses_2d.emplace_back(poses2d, dists);
+        }
+    }
+
+    // Check matches to old poses
+    double threshold = min_score - 0.2;
+    std::map<size_t, std::map<size_t, std::vector<size_t>>> scored_pasts;
+    if (!last_poses_3d.empty())
+    {
+        for (size_t i = 0; i < cameras.size(); ++i)
+        {
+            scored_pasts[i] = std::map<size_t, std::vector<size_t>>();
+            const std::vector<cv::Mat> &poses = poses_2d_mats[i];
+            const std::vector<cv::Mat> &last_poses = std::get<0>(last_poses_2d[i]);
+            const std::vector<cv::Mat> &dists = std::get<1>(last_poses_2d[i]);
+
+            for (size_t j = 0; j < last_poses.size(); ++j)
+            {
+                scored_pasts[i][j] = std::vector<size_t>();
+                const cv::Mat &last_pose = last_poses[j];
+
+                for (size_t k = 0; k < poses.size(); ++k)
+                {
+                    // Select core joints
+                    const cv::Mat &pose = poses[k];
+                    std::vector<int> dims = {(int)core_joint_idx.size(), 3};
+                    cv::Mat pose_core(dims, pose.type());
+                    for (size_t l = 0; l < core_joint_idx.size(); ++l)
+                    {
+                        size_t idx = core_joint_idx[l];
+                        pose.row(idx).copyTo(pose_core.row(l));
+                    }
+
+                    // Calculate score
+                    double score = calc_pose_score(pose_core, last_pose, dists[j], internal_cameras[i]);
+                    if (score > threshold)
+                    {
+                        scored_pasts[i][j].push_back(k);
+                    }
+                }
+            }
+        }
+    }
+
     // Create pairs of persons
     // Checks if the person was already matched to the last frame and if so only creates pairs
     // with those, else it creates all possible pairs
@@ -199,6 +267,27 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
                     int pid2 = std::accumulate(num_persons.begin(), num_persons.begin() + j, 0) + l;
                     bool match = false;
 
+                    if (!last_poses_3d.empty())
+                    {
+                        for (size_t m = 0; m < last_poses_3d.size(); ++m)
+                        {
+                            auto &smi = scored_pasts[i][m];
+                            auto &smj = scored_pasts[j][m];
+                            if (std::find(smi.begin(), smi.end(), k) != smi.end() &&
+                                std::find(smj.begin(), smj.end(), l) != smj.end())
+                            {
+                                match = true;
+                                all_pairs.emplace_back(
+                                    std::make_tuple(i, j, k, l), std::make_pair(pid1, pid2));
+                            }
+                            else if (std::find(smi.begin(), smi.end(), k) != smi.end() ||
+                                     std::find(smj.begin(), smj.end(), l) != smj.end())
+                            {
+                                match = true;
+                            }
+                        }
+                    }
+
                     if (!match)
                     {
                         all_pairs.emplace_back(
@@ -316,6 +405,7 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
         poses_3d.push_back(std::move(pose));
     }
 
+    last_poses_3d = all_merged_poses;
     return poses_3d;
 }
 
@@ -489,6 +579,45 @@ std::tuple<std::vector<cv::Mat>, std::vector<cv::Mat>> TriangulatorInternal::pro
 
 // =================================================================================================
 
+double TriangulatorInternal::calc_pose_score(
+    const cv::Mat &pose1,
+    const cv::Mat &pose2,
+    const cv::Mat &dist1,
+    const CameraInternal &icam)
+{
+    // Create mask for valid points
+    double min_score = 0.1;
+    cv::Mat mask = (pose1.col(2) > min_score) & (pose2.col(2) > min_score);
+    int valid_count = cv::countNonZero(mask);
+
+    // Drop if not enough valid points
+    if (valid_count < 3)
+    {
+        return 0.0;
+    }
+
+    // Calculate scores
+    double iscale = (icam.cam.width + icam.cam.height) / 2;
+    cv::Mat scores = score_projection(
+        pose1, pose2, dist1, mask, iscale);
+
+    // Drop lowest scores
+    int drop_k = static_cast<int>(pose1.rows * 0.2);
+    std::sort(scores.begin<double>(), scores.end<double>());
+
+    // Calculate final score
+    double score = 0.0;
+    for (int i = drop_k; i < scores.rows; i++)
+    {
+        score += scores.at<double>(i);
+    }
+    score /= (pose1.rows - drop_k);
+
+    return score;
+}
+
+// =================================================================================================
+
 cv::Mat TriangulatorInternal::score_projection(
     const cv::Mat &pose1,
     const cv::Mat &repro1,

spt/triangulator.hpp

@@ -66,6 +66,12 @@ private:
     std::tuple<std::vector<cv::Mat>, std::vector<cv::Mat>> project_poses(
         const std::vector<cv::Mat> &bodies3D, const CameraInternal &icam, bool calc_dists);
 
+    double calc_pose_score(
+        const cv::Mat &pose1,
+        const cv::Mat &pose2,
+        const cv::Mat &dist1,
+        const CameraInternal &icam);
+
     cv::Mat score_projection(
         const cv::Mat &pose1,
         const cv::Mat &repro1,

tests/test_interface.py

@@ -67,6 +67,7 @@ def main():
     )
     print("3D time:", time.time() - stime)
     print(np.array(poses_3d))
+    print("")
 
     # Load input data
     roomparams = [[0, -0.5, 1.2], [5.6, 6.4, 2.4]]
@@ -88,6 +89,16 @@ def main():
     )
     print("3D time:", time.time() - stime)
     print(np.array(poses_3d))
+    print("")
+
+    # Run again to test last pose cache
+    stime = time.time()
+    poses_3d = triangulator.triangulate_poses(
+        poses_2d, cameras, roomparams, joint_names
+    )
+    print("3D time:", time.time() - stime)
+    print(np.array(poses_3d))
+    print("")
 
 
 # ==================================================================================================