Change calculation of orientation

source/armarx/navigation/human/HumanTracker.cpp

@@ -9,6 +9,61 @@
 
 namespace armarx::navigation::human
 {
+    HumanTracker::DetectedHuman
+    convertHumanPoseToDetectedHuman(const DateTime& time, const armem::human::HumanPose& humanPose)
+    {
+        const std::map<std::string, armem::human::PoseKeypoint>& keypoints = humanPose.keypoints;
+        ARMARX_CHECK_NOT_EMPTY(keypoints);
+
+        // calculate the arithmetic mean of all keypoint positions
+        Eigen::Vector3f centerPos;
+        int size = 0;
+        for (const auto& [_, v] : keypoints)
+        {
+            if (v.positionGlobal.has_value())
+            {
+                centerPos += v.positionGlobal.value().toEigen();
+                size++;
+            }
+        }
+        centerPos /= size;
+
+        // calculate the yaw of the head keypoint if it exists
+        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);
+            Eigen::Vector3f rot3 = qhead._transformVector(vec);
+            Eigen::Vector2f rot2(rot3.y(), rot3.z());
+            if (rot2.norm() != 0)
+            {
+                rot2.normalize();
+                // calculate angle between e1 and rot2
+                yaw = atan2(rot2.y(), rot2.x() * rot2.y());
+            }
+            //old version using euler angles:
+            //yaw = humanPose //from all human pose keypoints
+            //          .keypoints
+            //          .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)
+        }
+
+        // create the new pose with the calculated position and yaw
+        core::Pose2D pose = core::Pose2D::Identity();
+        pose.translation() = conv::to2D(centerPos);
+        pose.linear() = Eigen::Rotation2Df(yaw).toRotationMatrix();
+
+        return {pose, humanPose.humanTrackingId, time, false};
+    }
+
+
     void
     HumanTracker::update(const Measurements& measurements)
     {
@@ -35,7 +90,7 @@ namespace armarx::navigation::human
         std::vector<DetectedHuman> newPoses =
             measurements.humanPoses |
             ranges::views::transform(
-                [measurements, this](const armem::human::HumanPose& humanPose) -> DetectedHuman {
+                [measurements](const armem::human::HumanPose& humanPose) -> DetectedHuman {
                     return convertHumanPoseToDetectedHuman(measurements.detectionTime, humanPose);
                 }) |
             ranges::to_vector;
@@ -73,45 +128,6 @@ namespace armarx::navigation::human
     }
 
 
-    HumanTracker::DetectedHuman
-    convertHumanPoseToDetectedHuman(const DateTime& time, const armem::human::HumanPose& humanPose)
-    {
-        const std::map<std::string, armem::human::PoseKeypoint>& keypoints = humanPose.keypoints;
-        ARMARX_CHECK_NOT_EMPTY(keypoints);
-
-        // calculate the arithmetic mean of all keypoint positions
-        Eigen::Vector3f centerPos;
-        int size = 0;
-        for (const auto& [_, v] : keypoints)
-        {
-            if (v.positionGlobal.has_value())
-            {
-                centerPos += v.positionGlobal.value().toEigen();
-                size++;
-            }
-        }
-        centerPos /= size;
-
-        // calculate the yaw of the head keypoint if it exists
-        double yaw = 0;
-        if (humanPose.keypoints.count("HEAD") > 0)
-        {
-            yaw = humanPose //from all human pose keypoints
-                      .keypoints
-                      .at("HEAD") //find the keypoint representing the head
-                      .orientationGlobal //get its global orientation
-                      ->toEigen()
-                      .eulerAngles(2, 1, 0)[0]; //and extract the yaw (rotation around z axis)
-        }
-
-        // create the new pose with the calculated position and yaw
-        core::Pose2D pose = core::Pose2D::Identity();
-        pose.translation() = conv::to2D(centerPos);
-        pose.linear() = Eigen::Rotation2Df(yaw).toRotationMatrix();
-
-        return {pose, humanPose.humanTrackingId, time, false};
-    }
-
     struct PosDistance
     {
         HumanTracker::TrackedHuman* oldHuman;

source/armarx/navigation/human/HumanTracker.h

@@ -118,16 +118,6 @@ namespace armarx::navigation::human
          * @param detectedHuman the detected human
          */
         void associate(TrackedHuman* tracked, DetectedHuman* detected);
-        /**
-         * @brief convertHumanPoseToDetectedHuman Calculates all information necessary for a
-         * DetectedHuman from the given HumanPose and returns a new detected human.
-         * @param time The point in time where the detection was made.
-         * @param humanPose The HumanPose that should be converted to a DetectedHuman.
-         * @return A new DetectedHuman according to the HumanPose
-         */
-        HumanTracker::DetectedHuman
-        convertHumanPoseToDetectedHuman(const DateTime& time,
-                                        const armem::human::HumanPose& humanPose);
 
     private:
         std::vector<TrackedHuman> trackedHumans;