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

Detect outliers by joint group distances.

Browse Source
This commit is contained in:
Daniel
2025-03-25 13:39:03 +01:00
parent 71968d06fd
commit 49a8855e03
2 changed files with 174 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

204
rpt/triangulator.cpp
View File

@ -884,6 +884,7 @@ std::vector<std::vector<std::array<float, 4>>> 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<std::vector<size_t>> 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<std::vector<std::array<float, 4>>> &poses,
const std::vector<std::string> &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<float, 4> center_head = {0.0, 0.0, 0.0, 0};
std::array<float, 4> center_foot_left = {0.0, 0.0, 0.0, 0};
std::array<float, 4> center_foot_right = {0.0, 0.0, 0.0, 0};
std::array<float, 4> center_hand_left = {0.0, 0.0, 0.0, 0};
std::array<float, 4> 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;
}
}
}
}
}
}

4
rpt/triangulator.hpp
View File

@ -138,6 +138,10 @@ private:
std::vector<std::vector<std::array<float, 4>>> &poses,
const std::vector<std::string> &joint_names);
void replace_far_joints(
std::vector<std::vector<std::array<float, 4>>> &poses,
const std::vector<std::string> &joint_names);
// Statistics
double num_calls = 0;
double total_time = 0;