diff --git a/spt/triangulator.cpp b/spt/triangulator.cpp index 43c3025..2d025e3 100644 --- a/spt/triangulator.cpp +++ b/spt/triangulator.cpp @@ -178,6 +178,74 @@ std::vector>> TriangulatorInternal::triangulat internal_cameras[i].update_projection_matrix(); } + // Project last 3D poses to 2D + std::vector, std::vector>> last_poses_2d; + if (!last_poses_3d.empty()) + { + // Select core joints + std::vector last_core_poses; + for (size_t i = 0; i < last_poses_3d.size(); ++i) + { + cv::Mat pose = last_poses_3d[i]; + std::vector 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>> scored_pasts; + if (!last_poses_3d.empty()) + { + for (size_t i = 0; i < cameras.size(); ++i) + { + scored_pasts[i] = std::map>(); + const std::vector &poses = poses_2d_mats[i]; + const std::vector &last_poses = std::get<0>(last_poses_2d[i]); + const std::vector &dists = std::get<1>(last_poses_2d[i]); + + for (size_t j = 0; j < last_poses.size(); ++j) + { + scored_pasts[i][j] = std::vector(); + 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 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>> 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>> 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> 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(pose1.rows * 0.2); + std::sort(scores.begin(), scores.end()); + + // Calculate final score + double score = 0.0; + for (int i = drop_k; i < scores.rows; i++) + { + score += scores.at(i); + } + score /= (pose1.rows - drop_k); + + return score; +} + +// ================================================================================================= + cv::Mat TriangulatorInternal::score_projection( const cv::Mat &pose1, const cv::Mat &repro1, diff --git a/spt/triangulator.hpp b/spt/triangulator.hpp index 4f8cd63..0987430 100644 --- a/spt/triangulator.hpp +++ b/spt/triangulator.hpp @@ -66,6 +66,12 @@ private: std::tuple, std::vector> project_poses( const std::vector &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, diff --git a/tests/test_interface.py b/tests/test_interface.py index 1a80ad9..1915977 100644 --- a/tests/test_interface.py +++ b/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("") # ==================================================================================================