From a073a12196730a0c3d03c6816d5dd2b67734cf87 Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Thu, 22 Sep 2022 12:48:24 +0200
Subject: [PATCH] Make getSortedDistances a class method
---
.../armarx/navigation/human/HumanTracker.cpp | 84 +++++++++----------
source/armarx/navigation/human/HumanTracker.h | 19 +++++
2 files changed, 57 insertions(+), 46 deletions(-)
diff --git a/source/armarx/navigation/human/HumanTracker.cpp b/source/armarx/navigation/human/HumanTracker.cpp
index 4cc4e75b..2332377e 100644
--- a/source/armarx/navigation/human/HumanTracker.cpp
+++ b/source/armarx/navigation/human/HumanTracker.cpp
@@ -73,51 +73,6 @@ namespace armarx::navigation::human
}
- struct PosDistance
- {
- HumanTracker::TrackedHuman* oldHuman;
- HumanTracker::DetectedHuman* newHuman;
- float distance;
- };
-
- std::vector<PosDistance>
- getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
- std::vector<HumanTracker::DetectedHuman>& newHumans)
- {
- std::vector<PosDistance> posDistances;
-
- for (auto& oldHuman : oldHumans)
- {
- if (oldHuman.associated)
- {
- continue;
- }
- for (auto& newHuman : newHumans)
- {
- if (newHuman.associated)
- {
- continue;
- }
- // calculate distance between every possible combination of tracked and detected
- // humans where none of them was associated
- posDistances.push_back(
- {&oldHuman,
- &newHuman,
- (newHuman.pose.translation() - oldHuman.humanFilter.get().pose.translation())
- .norm()});
- }
- }
-
- // sort the distances ascending by their numeric value
- std::sort(posDistances.begin(),
- posDistances.end(),
- [](const PosDistance& a, const PosDistance& b) -> bool
- { return a.distance < b.distance; });
-
- return posDistances;
- }
-
-
HumanTracker::DetectedHuman
HumanTracker::convertHumanPoseToDetectedHuman(const DateTime& time,
const armem::human::HumanPose& humanPose)
@@ -157,7 +112,7 @@ namespace armarx::navigation::human
//old version using euler angles:
//yaw = humanPose //from all human pose keypoints
// .keypoints
- // .at("HEAD") //find the keypoint representing the head
+ // .at(parameters.rotationKeypoint) //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
@@ -172,6 +127,43 @@ namespace armarx::navigation::human
return {pose, humanPose.humanTrackingId, time, false};
}
+ std::vector<HumanTracker::PosDistance>
+ HumanTracker::getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
+ std::vector<HumanTracker::DetectedHuman>& newHumans)
+ {
+ std::vector<PosDistance> posDistances;
+
+ for (auto& oldHuman : oldHumans)
+ {
+ if (oldHuman.associated)
+ {
+ continue;
+ }
+ for (auto& newHuman : newHumans)
+ {
+ if (newHuman.associated)
+ {
+ continue;
+ }
+ // calculate distance between every possible combination of tracked and detected
+ // humans where none of them was associated
+ posDistances.push_back(
+ {&oldHuman,
+ &newHuman,
+ (newHuman.pose.translation() - oldHuman.humanFilter.get().pose.translation())
+ .norm()});
+ }
+ }
+
+ // sort the distances ascending by their numeric value
+ std::sort(posDistances.begin(),
+ posDistances.end(),
+ [](const PosDistance& a, const PosDistance& b) -> bool
+ { return a.distance < b.distance; });
+
+ return posDistances;
+ }
+
void
HumanTracker::associateHumans(std::vector<DetectedHuman>& detectedHumans)
{
diff --git a/source/armarx/navigation/human/HumanTracker.h b/source/armarx/navigation/human/HumanTracker.h
index f2e3cd8e..1a83fb65 100644
--- a/source/armarx/navigation/human/HumanTracker.h
+++ b/source/armarx/navigation/human/HumanTracker.h
@@ -71,6 +71,13 @@ namespace armarx::navigation::human
bool associated;
};
+ struct PosDistance
+ {
+ HumanTracker::TrackedHuman* oldHuman;
+ HumanTracker::DetectedHuman* newHuman;
+ float distance;
+ };
+
struct Parameters
{
// the keypoint which should be used for calculating the rotation of the humans
@@ -115,6 +122,18 @@ namespace armarx::navigation::human
*/
DetectedHuman convertHumanPoseToDetectedHuman(const DateTime& time,
const armem::human::HumanPose& humanPose);
+ /**
+ * @brief getSortedDistances Returns a sorted vector of the distances between every possible
+ * combination (T, D) where T is an old, tracked human and D is a new, detected human and
+ * both of them are not already associated. The smallest distance will be the first entry in
+ * the vector
+ * @param oldHumans the old, tracked humans
+ * @param newHumans the new, detected humans
+ * @return the sorted vector of distances with references to the according humans
+ */
+ std::vector<PosDistance>
+ getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
+ std::vector<HumanTracker::DetectedHuman>& newHumans);
/**
* @brief HumanTracker::associateHumans Associates those tracked and detected humans that
* belong together.
--
GitLab