Print duration stats.

spt/triangulator.cpp

@@ -1,3 +1,4 @@
+#include <chrono>
 #include <numeric>
 
 #include <omp.h>
@@ -114,6 +115,10 @@ std::vector<std::vector<std::array<float, 4>>> TriangulatorInternal::triangulate
     const std::array<std::array<float, 3>, 2> &roomparams,
     const std::vector<std::string> &joint_names)
 {
+    auto ttime = std::chrono::high_resolution_clock::now();
+    auto stime = std::chrono::high_resolution_clock::now();
+    std::chrono::duration<float> elapsed;
+
     // Check inputs
     if (poses_2d.empty())
     {
@@ -183,6 +188,10 @@ std::vector<std::vector<std::array<float, 4>>> TriangulatorInternal::triangulate
                                   (size_t)std::distance(core_joints.begin(), it2)});
     }
 
+    elapsed = std::chrono::high_resolution_clock::now() - stime;
+    init_time += elapsed.count();
+    stime = std::chrono::high_resolution_clock::now();
+
     // Undistort 2D poses
     #pragma omp parallel for
     for (size_t i = 0; i < cameras.size(); ++i)
@@ -191,6 +200,10 @@ std::vector<std::vector<std::array<float, 4>>> TriangulatorInternal::triangulate
         internal_cameras[i].update_projection_matrix();
     }
 
+    elapsed = std::chrono::high_resolution_clock::now() - stime;
+    undistort_time += elapsed.count();
+    stime = std::chrono::high_resolution_clock::now();
+
     // 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())
@@ -222,6 +235,10 @@ std::vector<std::vector<std::array<float, 4>>> TriangulatorInternal::triangulate
         }
     }
 
+    elapsed = std::chrono::high_resolution_clock::now() - stime;
+    project_time += elapsed.count();
+    stime = std::chrono::high_resolution_clock::now();
+
     // Check matches to old poses
     float threshold = min_score - 0.2;
     std::map<size_t, std::map<size_t, std::vector<size_t>>> scored_pasts;
@@ -307,6 +324,10 @@ std::vector<std::vector<std::array<float, 4>>> TriangulatorInternal::triangulate
         }
     }
 
+    elapsed = std::chrono::high_resolution_clock::now() - stime;
+    match_time += elapsed.count();
+    stime = std::chrono::high_resolution_clock::now();
+
     // 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
@@ -380,6 +401,10 @@ std::vector<std::vector<std::array<float, 4>>> TriangulatorInternal::triangulate
         }
     }
 
+    elapsed = std::chrono::high_resolution_clock::now() - stime;
+    pairs_time += elapsed.count();
+    stime = std::chrono::high_resolution_clock::now();
+
     // Calculate pair scores
     std::vector<std::pair<cv::Mat, float>> all_scored_poses;
     all_scored_poses.resize(all_pairs.size());
@@ -413,6 +438,10 @@ std::vector<std::vector<std::array<float, 4>>> TriangulatorInternal::triangulate
         all_scored_poses[i] = std::make_pair(pose3d, score);
     }
 
+    elapsed = std::chrono::high_resolution_clock::now() - stime;
+    pair_scoring_time += elapsed.count();
+    stime = std::chrono::high_resolution_clock::now();
+
     // Drop low scoring poses
     std::vector<size_t> drop_indices;
     for (size_t i = 0; i < all_scored_poses.size(); ++i)
@@ -435,6 +464,10 @@ std::vector<std::vector<std::array<float, 4>>> TriangulatorInternal::triangulate
     std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> groups;
     groups = calc_grouping(all_pairs, all_scored_poses, min_score);
 
+    elapsed = std::chrono::high_resolution_clock::now() - stime;
+    grouping_time += elapsed.count();
+    stime = std::chrono::high_resolution_clock::now();
+
     // Calculate full 3D poses
     std::vector<cv::Mat> all_full_poses;
     all_full_poses.resize(all_pairs.size());
@@ -452,6 +485,10 @@ std::vector<std::vector<std::array<float, 4>>> TriangulatorInternal::triangulate
         all_full_poses[i] = (pose3d);
     }
 
+    elapsed = std::chrono::high_resolution_clock::now() - stime;
+    full_time += elapsed.count();
+    stime = std::chrono::high_resolution_clock::now();
+
     // Merge groups
     std::vector<cv::Mat> all_merged_poses;
     all_merged_poses.resize(groups.size());
@@ -471,6 +508,10 @@ std::vector<std::vector<std::array<float, 4>>> TriangulatorInternal::triangulate
         all_merged_poses[i] = (merged_pose);
     }
 
+    elapsed = std::chrono::high_resolution_clock::now() - stime;
+    merge_time += elapsed.count();
+    stime = std::chrono::high_resolution_clock::now();
+
     // Run post-processing steps
     std::vector<cv::Mat> final_poses_3d = all_merged_poses;
     add_extra_joints(final_poses_3d, joint_names);
@@ -478,6 +519,10 @@ std::vector<std::vector<std::array<float, 4>>> TriangulatorInternal::triangulate
     add_missing_joints(final_poses_3d, joint_names);
     last_poses_3d = final_poses_3d;
 
+    elapsed = std::chrono::high_resolution_clock::now() - stime;
+    post_time += elapsed.count();
+    stime = std::chrono::high_resolution_clock::now();
+
     // Convert to output format
     std::vector<std::vector<std::array<float, 4>>> poses_3d;
     poses_3d.reserve(final_poses_3d.size());
@@ -511,6 +556,13 @@ std::vector<std::vector<std::array<float, 4>>> TriangulatorInternal::triangulate
         }
     }
 
+    elapsed = std::chrono::high_resolution_clock::now() - stime;
+    convert_time += elapsed.count();
+
+    elapsed = std::chrono::high_resolution_clock::now() - ttime;
+    total_time += elapsed.count();
+    num_calls++;
+
     return poses_3d;
 }
 
@@ -523,6 +575,26 @@ void TriangulatorInternal::reset()
 
 // =================================================================================================
 
+void TriangulatorInternal::print_stats()
+{
+    std::cout << "Triangulator statistics:" << std::endl;
+    std::cout << " Number of calls: " << num_calls << std::endl;
+    std::cout << " Init time: " << init_time / num_calls << std::endl;
+    std::cout << " Undistort time: " << undistort_time / num_calls << std::endl;
+    std::cout << " Project time: " << project_time / num_calls << std::endl;
+    std::cout << " Match time: " << match_time / num_calls << std::endl;
+    std::cout << " Pairs time: " << pairs_time / num_calls << std::endl;
+    std::cout << " Pair scoring time: " << pair_scoring_time / num_calls << std::endl;
+    std::cout << " Grouping time: " << grouping_time / num_calls << std::endl;
+    std::cout << " Full time: " << full_time / num_calls << std::endl;
+    std::cout << " Merge time: " << merge_time / num_calls << std::endl;
+    std::cout << " Post time: " << post_time / num_calls << std::endl;
+    std::cout << " Convert time: " << convert_time / num_calls << std::endl;
+    std::cout << " Total time: " << total_time / num_calls << std::endl;
+}
+
+// =================================================================================================
+
 void TriangulatorInternal::undistort_poses(std::vector<cv::Mat> &poses, CameraInternal &icam)
 {
     int width = icam.cam.width;