Make convertHumanPoseToDetectedHuman a class method

source/armarx/navigation/human/HumanTracker.cpp

@@ -9,59 +9,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)
-        {
-            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.x(), rot3.y());
-            if (rot2.norm() != 0)
-            {
-                // calculate angle of rot2
-                yaw = atan2(rot2.y(), rot2.x());
-            }
-            //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)[0]; //and extract the yaw (rotation around 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)
     {
@@ -88,7 +35,7 @@ namespace armarx::navigation::human
         std::vector<DetectedHuman> newPoses =
             measurements.humanPoses |
             ranges::views::transform(
-                [measurements](const armem::human::HumanPose& humanPose) -> DetectedHuman {
+                [measurements, this](const armem::human::HumanPose& humanPose) -> DetectedHuman {
                     return convertHumanPoseToDetectedHuman(measurements.detectionTime, humanPose);
                 }) |
             ranges::to_vector;
@@ -170,6 +117,61 @@ namespace armarx::navigation::human
         return posDistances;
     }
 
+
+    HumanTracker::DetectedHuman
+    HumanTracker::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 specified keypoint representing the orientation if it exists
+        double yaw = 0;
+        if (humanPose.keypoints.count(parameters.rotationKeypoint) > 0)
+        {
+            const armem::human::PoseKeypoint& refKeypoint =
+                humanPose.keypoints.at(parameters.rotationKeypoint);
+            ARMARX_CHECK(refKeypoint.orientationGlobal.has_value());
+            Eigen::Quaternionf qhead = refKeypoint.orientationGlobal->toEigenQuaternion();
+            Eigen::Vector3f vec(1, 0, 0);
+            Eigen::Vector3f rot3 = qhead._transformVector(vec);
+            Eigen::Vector2f rot2(rot3.x(), rot3.y());
+            if (rot2.norm() != 0)
+            {
+                // calculate angle of rot2
+                yaw = atan2(rot2.y(), rot2.x());
+            }
+            //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)[0]; //and extract the yaw (rotation around 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::associateHumans(std::vector<DetectedHuman>& detectedHumans)
     {

source/armarx/navigation/human/HumanTracker.h

@@ -35,7 +35,7 @@
 
 namespace armarx::navigation::human
 {
-    using T = double; //TODO double or float?
+    using T = double;
     using Vector = Eigen::Matrix<T, 2, 1>;
     using SystemModelT = kalman_filter::SystemModelSO2xR2<T>;
 
@@ -73,6 +73,8 @@ namespace armarx::navigation::human
 
         struct Parameters
         {
+            // the keypoint which should be used for calculating the rotation of the humans
+            const std::string rotationKeypoint = "HEAD";
             // the duration after which tracked humans will be erased if no new measurement for
             // this human is found
             Duration maxTrackingAge = Duration::MilliSeconds(500);
@@ -104,6 +106,15 @@ namespace armarx::navigation::human
         void reset();
 
     private:
+        /**
+         * @brief convertHumanPoseToDetectedHuman Sets every parameter of a detected human according
+         * to the human pose and returns the new created detected human.
+         * @param time the time of detection
+         * @param humanPose the human pose representing the human
+         * @return the new created detected human
+         */
+        DetectedHuman convertHumanPoseToDetectedHuman(const DateTime& time,
+                                                      const armem::human::HumanPose& humanPose);
         /**
          * @brief HumanTracker::associateHumans Associates those tracked and detected humans that
          * belong together.
@@ -119,6 +130,7 @@ namespace armarx::navigation::human
          */
         void associate(TrackedHuman* tracked, DetectedHuman* detected);
 
+
     private:
         std::vector<TrackedHuman> trackedHumans;
         Parameters parameters;