From fa0a4da72df9b09dc98dcad2b1d1654d1870b581 Mon Sep 17 00:00:00 2001 From: Daniel Date: Mon, 24 Mar 2025 18:10:04 +0100 Subject: [PATCH] Optimized adjacent joint mapping. --- rpt/triangulator.cpp | 186 ++++++++++++++++++++++--------------------- 1 file changed, 95 insertions(+), 91 deletions(-) diff --git a/rpt/triangulator.cpp b/rpt/triangulator.cpp index 0abccf6..098e6ed 100644 --- a/rpt/triangulator.cpp +++ b/rpt/triangulator.cpp @@ -2223,131 +2223,135 @@ void TriangulatorInternal::add_missing_joints( {"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"}}, + {"foot_*_left", {"ankle_left"}}, + {"foot_*_right", {"ankle_right"}}, {"face_*", {"nose"}}, {"hand_*_left_*", {"wrist_left"}}, {"hand_*_right_*", {"wrist_right"}}}; + // Collect adjacent joint mappings + std::vector> adjacent_indices; + adjacent_indices.resize(joint_names.size()); + for (size_t j = 0; j < joint_names.size(); ++j) + { + 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()) + { + adname = jname; + } + + if (!adname.empty()) + { + auto it = adjacents.find(adname); + if (it != adjacents.end()) + { + for (const std::string &adj_name : it->second) + { + auto jt = joint_name_to_idx.find(adj_name); + if (jt != joint_name_to_idx.end()) + { + adjacent_indices[j].push_back(jt->second); + } + } + } + } + } + for (size_t i = 0; i < poses.size(); ++i) { auto &pose = poses[i]; size_t num_joints = pose.size(); - // Collect valid joint indices - std::vector valid_joint_idx; + // Compute body center as the mean of valid joints + std::array body_center = {0.0, 0.0, 0.0}; + size_t valid_count = 0; for (size_t j = 0; j < num_joints; ++j) { if (pose[j][3] > min_match_score) { - valid_joint_idx.push_back(j); + body_center[0] += pose[j][0]; + body_center[1] += pose[j][1]; + body_center[2] += pose[j][2]; + valid_count++; } } - - if (valid_joint_idx.empty()) + if (valid_count == 0) { continue; } - - // Compute body center as the mean of valid joints - std::array body_center = {0.0, 0.0, 0.0}; - for (size_t idx : valid_joint_idx) - { - body_center[0] += pose[idx][0]; - body_center[1] += pose[idx][1]; - body_center[2] += pose[idx][2]; - } + float inv_c = 1.0 / static_cast(valid_count); for (int j = 0; j < 3; ++j) { - body_center[j] /= static_cast(valid_joint_idx.size()); + body_center[j] *= inv_c; } // Iterate over each joint for (size_t j = 0; j < joint_names.size(); ++j) { - std::string adname = ""; - const std::string &jname = joint_names[j]; - - // Determine adjacent joint based on name patterns - if (jname.substr(0, 5) == "foot_" && jname.find("_left") != std::string::npos) - { - adname = "foot_*_left_*"; - } - else if (jname.substr(0, 5) == "foot_" && jname.find("_right") != std::string::npos) - { - adname = "foot_*_right_*"; - } - else if (jname.substr(0, 5) == "face_") - { - adname = "face_*"; - } - else if (jname.substr(0, 5) == "hand_" && jname.find("_left") != std::string::npos) - { - adname = "hand_*_left_*"; - } - else if (jname.substr(0, 5) == "hand_" && jname.find("_right") != std::string::npos) - { - adname = "hand_*_right_*"; - } - else if (adjacents.find(jname) != adjacents.end()) - { - adname = jname; - } - - if (adname == "") - { - // No adjacent joints defined for this joint - continue; - } - if (pose[j][3] == 0.0) { - if (adjacents.find(adname) != adjacents.end()) + // Joint is missing; attempt to estimate its position based on adjacent joints + const std::vector &adj_indices = adjacent_indices[j]; + std::array adjacent_position = {0.0, 0.0, 0.0}; + size_t adjacent_count = 0; + + for (size_t adj_idx : adj_indices) { - // Joint is missing; attempt to estimate its position based on adjacent joints - std::array adjacent_position = {0.0, 0.0, 0.0}; - size_t adjacent_count = 0; - - const std::vector &adjacent_joint_names = adjacents[adname]; - for (const std::string &adj_name : adjacent_joint_names) + if (adj_idx < pose.size() && pose[adj_idx][3] > 0.1) { - auto it = joint_name_to_idx.find(adj_name); - if (it != joint_name_to_idx.end()) - { - size_t adj_idx = it->second; - if (adj_idx < pose.size() && pose[adj_idx][3] > 0.1) - { - adjacent_position[0] += pose[adj_idx][0]; - adjacent_position[1] += pose[adj_idx][1]; - adjacent_position[2] += pose[adj_idx][2]; - ++adjacent_count; - } - } + adjacent_position[0] += pose[adj_idx][0]; + adjacent_position[1] += pose[adj_idx][1]; + adjacent_position[2] += pose[adj_idx][2]; + ++adjacent_count; + } + } + + if (adjacent_count > 0) + { + float inv_c = 1.0 / static_cast(adjacent_count); + for (size_t k = 0; k < 3; ++k) + { + adjacent_position[k] *= inv_c; } - if (adjacent_count > 0) - { - for (size_t k = 0; k < 3; ++k) - { - adjacent_position[k] /= static_cast(adjacent_count); - } - - // Update the missing joint's position - pose[j][0] = adjacent_position[0]; - pose[j][1] = adjacent_position[1]; - pose[j][2] = adjacent_position[2]; - } - else - { - // No valid adjacent joints, use body center - pose[j][0] = body_center[0]; - pose[j][1] = body_center[1]; - pose[j][2] = body_center[2]; - } + // Update the missing joint's position + pose[j][0] = adjacent_position[0]; + pose[j][1] = adjacent_position[1]; + pose[j][2] = adjacent_position[2]; } else { - // Adjacent joints not defined, use body center + // No valid adjacent joints, use body center pose[j][0] = body_center[0]; pose[j][1] = body_center[1]; pose[j][2] = body_center[2];