human tracker skeleton

source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp

+
 #include "HumanTracker.h"
 
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+
 namespace armarx::navigation::components::dynamic_scene_provider
 {
 
-HumanTracker::HumanTracker()
-{
+    void
+    HumanTracker::update(const Measurements& measurements)
+    {
+        trackedHumans.clear();
 
-}
+        for (const armem::human::HumanPose& humanPose : measurements.humanPoses)
+        {
 
+            const armem::human::Keypoint3DIdMap& keypoints = humanPose.keypoint3dMap;
+            ARMARX_CHECK_NOT_EMPTY(keypoints);
 
-void HumanTracker::update(const Measurements &measurements)
-{
-    trackedHumans.clear();
+            Eigen::Vector3f centerPos = Eigen::Vector3f::Zero();
+            for (const auto& [_, v] : keypoints)
+            {
+                centerPos += v.positionGlobal;
+            }
+            centerPos /= static_cast<float>(keypoints.size());
 
-    foreach (humanPose, measurements.humanPoses) {
+            core::Pose2D pose = core::Pose2D::Identity();
+            pose.translation() = Eigen::Vector2f{centerPos.x(), centerPos.y()};
+            pose.linear() = Eigen::Rotation2Df(0 /*angle*/).toRotationMatrix();
 
-        const armem::human::Keypoint3DIdMap& keypoints = humanPose.keypoint3dMap;
-        ARMARX_CHECK_NOT_EMPTY(keypoints);
+            const HumanTracker::TrackedHuman newHuman = {
+                .global_T_human = pose,
+                .linearVelocity = Eigen::Vector2f::Zero() //TODO more sophisticated guess
+            };
 
-        const Eigen::Vector3f centerPos = Eigen::Vector3f::Zero();
-        for (const auto& [_, v] : keypoints)
-        {
-            centerPos += v.positionGlobal;
-        }
-        centerPos /= static_cast<float>(keypoints.size());
-
-        const HumanTracker::TrackedHuman newHuman = HumanTracker::TrackedHuman{
-            .global_T_human = core::Pose2D{
-                .x = centerPos.x;
-                .y = centerPos.y;
-            };;
-            .linearVelocity = 0; //TODO more sophisticated guess
+            trackedHumans.push_back(newHuman);
         }
-        trackedHumans.push_back(newHuman);
     }
 
-
-    this->trackedHumans = HumanTracker::TrackedHuman{
-            .global_T_human = core::Pose2D{
-                .x = 0;
-                .y = 0;
-            };
-            .linearVelocity = 0;
+    const std::vector<HumanTracker::TrackedHuman>&
+    HumanTracker::getTrackedHumans() const
+    {
+        return trackedHumans;
     }
-}
-
-
-std::vector<HumanTracker::TrackedHuman> HumanTracker::getTrackedHumans() const
-{
-
-}
 
 
-void HumanTracker::reset()
-{
+    void
+    HumanTracker::reset()
+    {
+        trackedHumans.clear();
+    }
 
-}
 
-  
-}
+} // namespace armarx::navigation::components::dynamic_scene_provider