First tracker implementation.

scripts/test_skelda_dataset.cpp

@@ -17,6 +17,7 @@ using json = nlohmann::json;
 
 #include "/RapidPoseTriangulation/rpt/camera.hpp"
 #include "/RapidPoseTriangulation/rpt/interface.hpp"
+#include "/RapidPoseTriangulation/rpt/tracker.hpp"
 #include "/RapidPoseTriangulation/scripts/utils_2d_pose.hpp"
 #include "/RapidPoseTriangulation/scripts/utils_pipeline.hpp"
 
@@ -121,6 +122,7 @@ int main(int argc, char **argv)
     const float min_match_score = config["min_match_score"];
     const size_t min_group_size = config["min_group_size"];
     const int take_interval = config["take_interval"];
+    const float ifps = config["fps"];
 
     // Load 2D model
     bool use_wb = utils_pipeline::use_whole_body(whole_body);
@@ -131,6 +133,9 @@ int main(int argc, char **argv)
     // Load 3D model
     std::unique_ptr<Triangulator> tri_model = std::make_unique<Triangulator>(
         min_match_score, min_group_size);
+    const float max_distance = 0.3 + 2.0 / ifps;
+    std::unique_ptr<PoseTracker> pose_tracker = std::make_unique<PoseTracker>(
+        max_distance);
 
     // Timers
     size_t time_count = dataset.size();
@@ -138,10 +143,12 @@ int main(int argc, char **argv)
     std::vector<double> times_debayer;
     std::vector<double> times_pose2d;
     std::vector<double> times_pose3d;
+    std::vector<double> times_tracks;
     times_image.reserve(time_count);
     times_debayer.reserve(time_count);
     times_pose2d.reserve(time_count);
     times_pose3d.reserve(time_count);
+    times_tracks.reserve(time_count);
     size_t print_steps = (size_t)std::floor((float)time_count / 100.0f);
     print_steps = std::max((size_t)1, print_steps);
 
@@ -214,6 +221,7 @@ int main(int argc, char **argv)
         {
             // Reset last poses if scene changes
             tri_model->reset();
+            pose_tracker->reset();
             old_scene = item["scene"];
         }
 
@@ -241,6 +249,19 @@ int main(int argc, char **argv)
         elapsed = std::chrono::high_resolution_clock::now() - stime;
         times_pose3d.push_back(elapsed.count());
 
+        stime = std::chrono::high_resolution_clock::now();
+        double ts = ((int)item["index"]) / ifps;
+        auto pose_tracks = pose_tracker->track_poses(poses_3d, joint_names_2d, ts);
+        std::vector<std::vector<std::array<float, 4>>> poses_3d_refined;
+        for (size_t j = 0; j < pose_tracks.size(); j++)
+        {
+            auto &pose = std::get<1>(pose_tracks[j]);
+            poses_3d_refined.push_back(pose);
+        }
+        poses_3d = poses_3d_refined;
+        elapsed = std::chrono::high_resolution_clock::now() - stime;
+        times_tracks.push_back(elapsed.count());
+
         all_poses_3d.push_back(std::move(poses_3d));
         all_ids.push_back(item["id"].get<std::string>());
         old_id = item["index"];
@@ -254,23 +275,27 @@ int main(int argc, char **argv)
     double time_debayer = 0.0;
     double time_pose2d = 0.0;
     double time_pose3d = 0.0;
+    double time_tracks = 0.0;
     for (size_t i = warmup; i < time_count; i++)
     {
         time_image += times_image[i];
         time_debayer += times_debayer[i];
         time_pose2d += times_pose2d[i];
         time_pose3d += times_pose3d[i];
+        time_tracks += times_tracks[i];
     }
     double avg_time_image = time_image / (time_count - warmup);
     double avg_time_debayer = time_debayer / (time_count - warmup);
     double avg_time_pose2d = time_pose2d / (time_count - warmup);
     double avg_time_pose3d = time_pose3d / (time_count - warmup);
-    double fps = 1.0 / (avg_time_debayer + avg_time_pose2d + avg_time_pose3d);
+    double avg_time_tracks = time_tracks / (time_count - warmup);
+    double fps = 1.0 / (avg_time_debayer + avg_time_pose2d + avg_time_pose3d + avg_time_tracks);
     std::cout << "{\n"
               << "  \"img_loading\": " << avg_time_image << ",\n"
               << "  \"demosaicing\": " << avg_time_debayer << ",\n"
               << "  \"avg_time_2d\": " << avg_time_pose2d << ",\n"
               << "  \"avg_time_3d\": " << avg_time_pose3d << ",\n"
+              << "  \"time_tracks\": " << avg_time_tracks << ",\n"
               << "  \"fps\": " << fps << "\n"
               << "}" << std::endl;
     tri_model->print_stats();

scripts/test_skelda_dataset.py

@@ -55,6 +55,7 @@ datasets = {
     "human36m": {
         "path": "/datasets/human36m/skelda/pose_test.json",
         "take_interval": 5,
+        "fps": 50,
         "min_match_score": 0.95,
         "min_group_size": 1,
         "min_bbox_score": 0.4,
@@ -84,6 +85,7 @@ datasets = {
     },
     "campus": {
         "path": "/datasets/campus/skelda/test.json",
+        "fps": 20,
         "take_interval": 1,
         "min_match_score": 0.92,
         "min_bbox_score": 0.5,
@@ -91,6 +93,7 @@ datasets = {
     "shelf": {
         "path": "/datasets/shelf/skelda/test.json",
         "take_interval": 1,
+        "fps": 20,
         "min_match_score": 0.95,
         "min_group_size": 2,
     },
@@ -346,6 +349,7 @@ def main():
         "batch_poses": batch_poses,
         "whole_body": whole_body,
         "take_interval": datasets[dataset_use]["take_interval"],
+        "fps": datasets[dataset_use]["fps"],
     }
     utils_pipeline.save_json(config, config_path)