Fix calculation of humans rotation

source/armarx/navigation/human/HumanTracker.cpp

@@ -32,18 +32,16 @@ namespace armarx::navigation::human
         double yaw = 0;
         if (humanPose.keypoints.count("HEAD") > 0)
         {
-            Eigen::Quaternionf qhead =
-                humanPose.keypoints.at("HEAD").orientationGlobal->toEigenQuaternion();
-            //TODO not quite sure if the quaternion operates this way as it is not further defined
-            // on https://learn.microsoft.com/de-de/azure/kinect-dk/body-joints
-            Eigen::Vector3f vec(0, 1, 0);
+            const armem::human::PoseKeypoint& headKeypoint = humanPose.keypoints.at("HEAD");
+            ARMARX_CHECK(headKeypoint.orientationGlobal.has_value());
+            Eigen::Quaternionf qhead = headKeypoint.orientationGlobal->toEigenQuaternion();
+            Eigen::Vector3f vec(1, 0, 0);
             Eigen::Vector3f rot3 = qhead._transformVector(vec);
-            Eigen::Vector2f rot2(rot3.y(), rot3.z());
+            Eigen::Vector2f rot2(rot3.x(), rot3.y());
             if (rot2.norm() != 0)
             {
-                rot2.normalize();
-                // calculate angle between e1 and rot2
-                yaw = atan2(rot2.y(), rot2.x() * rot2.y());
+                // calculate angle of rot2
+                yaw = atan2(rot2.y(), rot2.x());
             }
             //old version using euler angles:
             //yaw = humanPose //from all human pose keypoints
@@ -51,8 +49,8 @@ namespace armarx::navigation::human
             //          .at("HEAD") //find the keypoint representing the head
             //          .orientationGlobal //get its global orientation
             //          ->toEigen()
-            //          .eulerAngles(2, 1, 0)[2]; //and extract the yaw (rotation around x axis
-            //                                          should be z axis in global coordinates)
+            //          .eulerAngles(2, 1, 0)[0]; //and extract the yaw (rotation around z axis in
+            //                                                                  global coordinates)
         }
 
         // create the new pose with the calculated position and yaw