Optimized adjacent joint mapping.

rpt/triangulator.cpp

@@ -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_*_left", {"ankle_left"}},
-        {"foot_*_right_*", {"ankle_right"}},
+        {"foot_*_right", {"ankle_right"}},
         {"face_*", {"nose"}},
         {"hand_*_left_*", {"wrist_left"}},
         {"hand_*_right_*", {"wrist_right"}}};
 
-    for (size_t i = 0; i < poses.size(); ++i)
+    // Collect adjacent joint mappings
-    {
+    std::vector<std::vector<size_t>> adjacent_indices;
-        auto &pose = poses[i];
+    adjacent_indices.resize(joint_names.size());
-        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
     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_*";
+            if (jname.find("_left") != std::string::npos)
-            }
-            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)
+            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
+            auto it = adjacents.find(adname);
-                continue;
+            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 (size_t adj_idx : adj_indices)
-                    for (const std::string &adj_name : adjacent_joint_names)
                 {
-                        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;