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

144
rpt/triangulator.cpp
View File

@ -2223,98 +2223,110 @@ 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"}}};
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<size_t> valid_joint_idx;
for (size_t j = 0; j < num_joints; ++j)
{
if (pose[j][3] > min_match_score)
{
valid_joint_idx.push_back(j);
}
}
if (valid_joint_idx.empty())
{
continue;
}
// 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)
{
body_center[j] /= static_cast<float>(valid_joint_idx.size());
}
// Iterate over each joint
// 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 (jname.substr(0, 5) == "foot_" && jname.find("_left") != std::string::npos)
if (nlen >= 15 && jname.compare(0, 5, "hand_") == 0)
{
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)
if (jname.find("_left") != std::string::npos)
{
adname = "hand_*_left_*";
}
else if (jname.substr(0, 5) == "hand_" && jname.find("_right") != std::string::npos)
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 == "")
if (!adname.empty())
{
// No adjacent joints defined for this joint
continue;
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();
// Compute body center as the mean of valid joints
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)
{
if (pose[j][3] > min_match_score)
{
body_center[0] += pose[j][0];
body_center[1] += pose[j][1];
body_center[2] += pose[j][2];
valid_count++;
}
}
if (valid_count == 0)
{
continue;
}
float inv_c = 1.0 / static_cast<float>(valid_count);
for (int j = 0; j < 3; ++j)
{
body_center[j] *= inv_c;
}
// Iterate over each joint
for (size_t j = 0; j < joint_names.size(); ++j)
{
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;
const std::vector<std::string> &adjacent_joint_names = adjacents[adname];
for (const std::string &adj_name : adjacent_joint_names)
for (size_t adj_idx : adj_indices)
{
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];
@ -2323,13 +2335,13 @@ void TriangulatorInternal::add_missing_joints(
++adjacent_count;
}
}
}
if (adjacent_count > 0)
{
float inv_c = 1.0 / static_cast<float>(adjacent_count);
for (size_t k = 0; k < 3; ++k)
{
adjacent_position[k] /= static_cast<float>(adjacent_count);
adjacent_position[k] *= inv_c;
}
// Update the missing joint's position
@ -2344,14 +2356,6 @@ void TriangulatorInternal::add_missing_joints(
pose[j][1] = body_center[1];
pose[j][2] = body_center[2];
}
}
else
{
// Adjacent joints not defined, use body center
pose[j][0] = body_center[0];
pose[j][1] = body_center[1];
pose[j][2] = body_center[2];
}
// Set a low confidence score
pose[j][3] = 0.1;