diff --git a/source/armarx/navigation/human/HumanTracker.cpp b/source/armarx/navigation/human/HumanTracker.cpp
index 0d5d85fef0fe9092208943bf752ef310aadfb356..4cc4e75bb08f5fd60f3533e48f4ddad34f78d01c 100644
--- a/source/armarx/navigation/human/HumanTracker.cpp
+++ b/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)
{
diff --git a/source/armarx/navigation/human/HumanTracker.h b/source/armarx/navigation/human/HumanTracker.h
index ffd0bf2b7d08527796a94c61164a363e4c4b7bbf..f2e3cd8e09459b73015940fb5a78a63ccb9ecab9 100644
--- a/source/armarx/navigation/human/HumanTracker.h
+++ b/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;