Extract rotation from human pose keypoints

source/armarx/navigation/human/HumanTracker.cpp

@@ -30,8 +30,18 @@ namespace armarx::navigation::human
 
         core::Pose2D pose = core::Pose2D::Identity();
         pose.translation() = conv::to2D(centerPos);
-        //TODO: angle
-        pose.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+        double yaw = 0;
+        if (humanPose.keypoints.find("HEAD") != humanPose.keypoints.end())
+        {
+            yaw = humanPose //from all human pose keypoints
+                      .keypoints
+                      .find("HEAD") //find the keypoint representing the head
+                      ->second
+                      .orientationGlobal //get its global orientation
+                      ->toEigen()
+                      .eulerAngles(2, 1, 0)[0]; //and extract the yaw (rotation around z axis)
+        }
+        pose.linear() = Eigen::Rotation2Df(yaw).toRotationMatrix();
 
         return {pose, humanPose.humanTrackingId, time, false};
     }