From 49a8855e03eace292f248422d970a3c2b91ce8ae Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 25 Mar 2025 13:39:03 +0100 Subject: [PATCH] Detect outliers by joint group distances. --- rpt/triangulator.cpp | 204 +++++++++++++++++++++++++++++++++++-------- rpt/triangulator.hpp | 4 + 2 files changed, 174 insertions(+), 34 deletions(-) diff --git a/rpt/triangulator.cpp b/rpt/triangulator.cpp index a538e41..005bcb1 100644 --- a/rpt/triangulator.cpp +++ b/rpt/triangulator.cpp @@ -884,6 +884,7 @@ std::vector>> TriangulatorInternal::triangulate add_extra_joints(final_poses_3d, joint_names); filter_poses(final_poses_3d, roomparams, core_joint_idx, core_limbs_idx); add_missing_joints(final_poses_3d, joint_names); + replace_far_joints(final_poses_3d, joint_names); // Store final result for use in the next frame. last_poses_3d = final_poses_3d; @@ -2224,12 +2225,7 @@ void TriangulatorInternal::add_missing_joints( {"wrist_right", {"elbow_right"}}, {"wrist_left", {"elbow_left"}}, {"nose", {"shoulder_middle", "shoulder_right", "shoulder_left"}}, - {"head", {"shoulder_middle", "shoulder_right", "shoulder_left"}}, - {"foot_*_left", {"ankle_left"}}, - {"foot_*_right", {"ankle_right"}}, - {"face_*", {"nose"}}, - {"hand_*_left_*", {"wrist_left"}}, - {"hand_*_right_*", {"wrist_right"}}}; + {"head", {"shoulder_middle", "shoulder_right", "shoulder_left"}}}; // Collect adjacent joint mappings std::vector> adjacent_indices; @@ -2238,36 +2234,9 @@ void TriangulatorInternal::add_missing_joints( { std::string adname = ""; const std::string &jname = joint_names[j]; - size_t nlen = jname.size(); // Determine adjacent joint based on name patterns - if (nlen >= 15 && jname.compare(0, 5, "hand_") == 0) - { - if (jname.find("_left") != std::string::npos) - { - adname = "hand_*_left_*"; - } - else if (jname.find("_right") != std::string::npos) - { - adname = "hand_*_right_*"; - } - } - else if (nlen >= 14 && jname.compare(0, 5, "foot_") == 0) - { - if (jname.find("_left") != std::string::npos) - { - adname = "foot_*_left"; - } - else if (jname.find("_right") != std::string::npos) - { - adname = "foot_*_right"; - } - } - else if (nlen >= 11 && jname.compare(0, 5, "face_") == 0) - { - adname = "face_*"; - } - else if (adjacents.find(jname) != adjacents.end()) + if (adjacents.find(jname) != adjacents.end()) { adname = jname; } @@ -2365,3 +2334,170 @@ void TriangulatorInternal::add_missing_joints( } } } + +// ================================================================================================= + +void TriangulatorInternal::replace_far_joints( + std::vector>> &poses, + const std::vector &joint_names) +{ + for (size_t i = 0; i < poses.size(); ++i) + { + auto &pose = poses[i]; + size_t num_full_joints = pose.size(); + + // Calculate the average position of pose parts + std::array center_head = {0.0, 0.0, 0.0, 0}; + std::array center_foot_left = {0.0, 0.0, 0.0, 0}; + std::array center_foot_right = {0.0, 0.0, 0.0, 0}; + std::array center_hand_left = {0.0, 0.0, 0.0, 0}; + std::array center_hand_right = {0.0, 0.0, 0.0, 0}; + for (size_t j = 0; j < num_full_joints; ++j) + { + const std::string &jname = joint_names[j]; + float offset2 = (1.0 - min_match_score) * 2; + float min_score = this->min_match_score - offset2; + + if (pose[j][3] > min_score) + { + if (jname.compare(0, 4, "face_") == 0 || jname == "nose" || jname == "eye_left" || + jname == "eye_right" || jname == "ear_left" || jname == "ear_right") + { + center_head[0] += pose[j][0]; + center_head[1] += pose[j][1]; + center_head[2] += pose[j][2]; + center_head[3] += 1.0; + } + else if (jname.compare(0, 5, "foot_") == 0 || jname.compare(0, 6, "ankle_") == 0) + { + if (jname.find("_left") != std::string::npos) + { + center_foot_left[0] += pose[j][0]; + center_foot_left[1] += pose[j][1]; + center_foot_left[2] += pose[j][2]; + center_foot_left[3] += 1.0; + } + else if (jname.find("_right") != std::string::npos) + { + center_foot_right[0] += pose[j][0]; + center_foot_right[1] += pose[j][1]; + center_foot_right[2] += pose[j][2]; + center_foot_right[3] += 1.0; + } + } + else if (jname.compare(0, 5, "hand_") == 0 || jname.compare(0, 6, "wrist_") == 0) + { + if (jname.find("_left") != std::string::npos) + { + center_hand_left[0] += pose[j][0]; + center_hand_left[1] += pose[j][1]; + center_hand_left[2] += pose[j][2]; + center_hand_left[3] += 1.0; + } + else if (jname.find("_right") != std::string::npos) + { + center_hand_right[0] += pose[j][0]; + center_hand_right[1] += pose[j][1]; + center_hand_right[2] += pose[j][2]; + center_hand_right[3] += 1.0; + } + } + } + } + for (size_t k = 0; k < 3; k++) + { + center_head[k] /= center_head[3]; + center_foot_left[k] /= center_foot_left[3]; + center_foot_right[k] /= center_foot_right[3]; + center_hand_left[k] /= center_hand_left[3]; + center_hand_right[k] /= center_hand_right[3]; + } + + // Drop joints that are too far from the part center + const float max_dist_head_sq = 0.20 * 0.20; + const float max_dist_foot_sq = 0.25 * 0.25; + const float max_dist_hand_sq = 0.20 * 0.20; + for (size_t j = 0; j < num_full_joints; ++j) + { + const std::string &jname = joint_names[j]; + + if (jname.compare(0, 4, "face_") == 0 || jname == "nose" || jname == "eye_left" || + jname == "eye_right" || jname == "ear_left" || jname == "ear_right") + { + float dx = pose[j][0] - center_head[0]; + float dy = pose[j][1] - center_head[1]; + float dz = pose[j][2] - center_head[2]; + float dist_sq = dx * dx + dy * dy + dz * dz; + if ((pose[j][3] > 0.0 && dist_sq > max_dist_head_sq) || pose[j][3] == 0.0) + { + pose[j][0] = center_head[0]; + pose[j][1] = center_head[1]; + pose[j][2] = center_head[2]; + pose[j][3] = 0.1; + } + } + else if (jname.compare(0, 5, "foot_") == 0 || jname.compare(0, 6, "ankle_") == 0) + { + if (jname.find("_left") != std::string::npos) + { + float dx = pose[j][0] - center_foot_left[0]; + float dy = pose[j][1] - center_foot_left[1]; + float dz = pose[j][2] - center_foot_left[2]; + float dist_sq = dx * dx + dy * dy + dz * dz; + if ((pose[j][3] > 0.0 && dist_sq > max_dist_foot_sq) || pose[j][3] == 0.0) + { + pose[j][0] = center_foot_left[0]; + pose[j][1] = center_foot_left[1]; + pose[j][2] = center_foot_left[2]; + pose[j][3] = 0.1; + } + } + else if (jname.find("_right") != std::string::npos) + { + float dx = pose[j][0] - center_foot_right[0]; + float dy = pose[j][1] - center_foot_right[1]; + float dz = pose[j][2] - center_foot_right[2]; + float dist_sq = dx * dx + dy * dy + dz * dz; + if ((pose[j][3] > 0.0 && dist_sq > max_dist_foot_sq) || pose[j][3] == 0.0) + { + pose[j][0] = center_foot_right[0]; + pose[j][1] = center_foot_right[1]; + pose[j][2] = center_foot_right[2]; + pose[j][3] = 0.1; + } + } + } + else if (jname.compare(0, 5, "hand_") == 0 || jname.compare(0, 6, "wrist_") == 0) + { + if (jname.find("_left") != std::string::npos) + { + float dx = pose[j][0] - center_hand_left[0]; + float dy = pose[j][1] - center_hand_left[1]; + float dz = pose[j][2] - center_hand_left[2]; + float dist_sq = dx * dx + dy * dy + dz * dz; + if ((pose[j][3] > 0.0 && dist_sq > max_dist_hand_sq) || pose[j][3] == 0.0) + { + pose[j][0] = center_hand_left[0]; + pose[j][1] = center_hand_left[1]; + pose[j][2] = center_hand_left[2]; + pose[j][3] = 0.1; + } + } + else if (jname.find("_right") != std::string::npos) + { + float dx = pose[j][0] - center_hand_right[0]; + float dy = pose[j][1] - center_hand_right[1]; + float dz = pose[j][2] - center_hand_right[2]; + float dist_sq = dx * dx + dy * dy + dz * dz; + if ((pose[j][3] > 0.0 && dist_sq > max_dist_hand_sq) || pose[j][3] == 0.0) + { + pose[j][0] = center_hand_right[0]; + pose[j][1] = center_hand_right[1]; + pose[j][2] = center_hand_right[2]; + pose[j][3] = 0.1; + } + } + } + } + } +} diff --git a/rpt/triangulator.hpp b/rpt/triangulator.hpp index b87aefa..36b724a 100644 --- a/rpt/triangulator.hpp +++ b/rpt/triangulator.hpp @@ -138,6 +138,10 @@ private: std::vector>> &poses, const std::vector &joint_names); + void replace_far_joints( + std::vector>> &poses, + const std::vector &joint_names); + // Statistics double num_calls = 0; double total_time = 0;