crosstyan/RapidPoseTriangulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Optimized adjacent joint mapping.

Browse Source
This commit is contained in:
Daniel
2025-03-24 18:10:04 +01:00
parent 0841713e1a
commit fa0a4da72d
1 changed files with 95 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files

186
rpt/triangulator.cpp
View File

@ -2223,131 +2223,135 @@ void TriangulatorInternal::add_missing_joints(
{"wrist_left", {"elbow_left"}}, {"wrist_left", {"elbow_left"}},
{"nose", {"shoulder_middle", "shoulder_right", "shoulder_left"}}, {"nose", {"shoulder_middle", "shoulder_right", "shoulder_left"}},
{"head", {"shoulder_middle", "shoulder_right", "shoulder_left"}}, {"head", {"shoulder_middle", "shoulder_right", "shoulder_left"}},
{"foot_*_left_*", {"ankle_left"}}, {"foot_*_left", {"ankle_left"}},
{"foot_*_right_*", {"ankle_right"}}, {"foot_*_right", {"ankle_right"}},
{"face_*", {"nose"}}, {"face_*", {"nose"}},
{"hand_*_left_*", {"wrist_left"}}, {"hand_*_left_*", {"wrist_left"}},
{"hand_*_right_*", {"wrist_right"}}}; {"hand_*_right_*", {"wrist_right"}}};
// Collect adjacent joint mappings
std::vector<std::vector<size_t>> 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) for (size_t i = 0; i < poses.size(); ++i)
{ {
auto &pose = poses[i]; auto &pose = poses[i];
size_t num_joints = pose.size(); size_t num_joints = pose.size();
// Collect valid joint indices // Compute body center as the mean of valid joints
std::vector<size_t> valid_joint_idx; std::array<float, 3> body_center = {0.0, 0.0, 0.0};
size_t valid_count = 0;
for (size_t j = 0; j < num_joints; ++j) for (size_t j = 0; j < num_joints; ++j)
{ {
if (pose[j][3] > min_match_score) 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_count == 0)
if (valid_joint_idx.empty())
{ {
continue; continue;
} }
float inv_c = 1.0 / static_cast<float>(valid_count);
// Compute body center as the mean of valid joints
std::array<float, 3> 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];
}
for (int j = 0; j < 3; ++j) for (int j = 0; j < 3; ++j)
{ {
body_center[j] /= static_cast<float>(valid_joint_idx.size()); body_center[j] *= inv_c;
} }
// Iterate over each joint // Iterate over each joint
for (size_t j = 0; j < joint_names.size(); ++j) 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 (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<size_t> &adj_indices = adjacent_indices[j];
std::array<float, 3> 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 if (adj_idx < pose.size() && pose[adj_idx][3] > 0.1)
std::array<float, 3> adjacent_position = {0.0, 0.0, 0.0};
size_t adjacent_count = 0;
const std::vector<std::string> &adjacent_joint_names = adjacents[adname];
for (const std::string &adj_name : adjacent_joint_names)
{ {
auto it = joint_name_to_idx.find(adj_name); adjacent_position[0] += pose[adj_idx][0];
if (it != joint_name_to_idx.end()) adjacent_position[1] += pose[adj_idx][1];
{ adjacent_position[2] += pose[adj_idx][2];
size_t adj_idx = it->second; ++adjacent_count;
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]; if (adjacent_count > 0)
adjacent_position[2] += pose[adj_idx][2]; {
++adjacent_count; float inv_c = 1.0 / static_cast<float>(adjacent_count);
} for (size_t k = 0; k < 3; ++k)
} {
adjacent_position[k] *= inv_c;
} }
if (adjacent_count > 0) // Update the missing joint's position
{ pose[j][0] = adjacent_position[0];
for (size_t k = 0; k < 3; ++k) pose[j][1] = adjacent_position[1];
{ pose[j][2] = adjacent_position[2];
adjacent_position[k] /= static_cast<float>(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];
}
} }
else else
{ {
// Adjacent joints not defined, use body center // No valid adjacent joints, use body center
pose[j][0] = body_center[0]; pose[j][0] = body_center[0];
pose[j][1] = body_center[1]; pose[j][1] = body_center[1];
pose[j][2] = body_center[2]; pose[j][2] = body_center[2];