diff --git a/source/armarx/navigation/human/HumanFilter.h b/source/armarx/navigation/human/HumanFilter.h
index 14be9eb1298ec57377c1b8d0d0b3878e53bd1bce..749750d422a7b1d44f2c0c0207314a7065dd42bd 100644
--- a/source/armarx/navigation/human/HumanFilter.h
+++ b/source/armarx/navigation/human/HumanFilter.h
@@ -30,6 +30,11 @@
namespace armarx::navigation::human
{
+ /**
+ * @brief The HumanFilter class can be used to track and filter the state of a single human. It
+ * hides implementation detail on how the filtering is done. New information about the human can
+ * be fed by using the update method. The human itself can be obtained using the get method.
+ */
class HumanFilter
{
diff --git a/source/armarx/navigation/human/HumanTracker.cpp b/source/armarx/navigation/human/HumanTracker.cpp
index e82ecf9e20a7577dc20798e33b83507133d46a9c..dd6ecfc45ee131aff39242ef78668c47b5337a56 100644
--- a/source/armarx/navigation/human/HumanTracker.cpp
+++ b/source/armarx/navigation/human/HumanTracker.cpp
@@ -9,54 +9,28 @@
namespace armarx::navigation::human
{
-
- HumanTracker::DetectedHuman
- convertHumanPoseToPosition(const DateTime& time, const armem::human::HumanPose& humanPose)
- {
- const std::map<std::string, armem::human::PoseKeypoint>& keypoints = humanPose.keypoints;
- ARMARX_CHECK_NOT_EMPTY(keypoints);
-
- Eigen::Vector3f centerPos;
- int size = 0;
- for (const auto& [_, v] : keypoints)
- {
- if (v.positionGlobal.has_value())
- {
- centerPos += v.positionGlobal.value().toEigen();
- size++;
- }
- }
- centerPos /= size;
-
- core::Pose2D pose = core::Pose2D::Identity();
- pose.translation() = conv::to2D(centerPos);
- double yaw = 0;
- if (humanPose.keypoints.find("HEAD") != humanPose.keypoints.end())
- {
- yaw = humanPose //from all human pose keypoints
- .keypoints
- .find("HEAD") //find the keypoint representing the head
- ->second
- .orientationGlobal //get its global orientation
- ->toEigen()
- .eulerAngles(2, 1, 0)[0]; //and extract the yaw (rotation around z axis)
- }
- pose.linear() = Eigen::Rotation2Df(yaw).toRotationMatrix();
-
- return {pose, humanPose.humanTrackingId, time, false};
- }
-
+ /**
+ * @brief HumanTracker::update Updates the tracked humans with the measurements. When a
+ * measurement is close enough to an existing tracked human, they are associated, otherwise a
+ * new tracked human is created. Tracked humans that were not associated with a new measurement
+ * for a specified amount of time are removed. New associated measurements for a tracked human
+ * are always filtered to provide a less noisy state.
+ * @param measurements the new measurements of the environment
+ */
void
HumanTracker::update(const Measurements& measurements)
{
+ // iterate over all existing tracked humans
for (auto it = trackedHumans.begin(); it != trackedHumans.end();)
{
auto& human = *it;
+ // when the tracked human recieved no new measurement for too long, remove it
if ((measurements.detectionTime - human.humanFilter.get().detectionTime) >=
parameters.maxTrackingAge)
{
it = trackedHumans.erase(it);
}
+ // otherwise the tracked human is prepared for association at the current point in time
else
{
human.associated = false;
@@ -65,19 +39,22 @@ namespace armarx::navigation::human
}
}
+ // calculate the poses according to the new received measurements
std::vector<DetectedHuman> newPoses =
measurements.humanPoses |
ranges::views::transform(
- [measurements](const armem::human::HumanPose& humanPose) -> DetectedHuman
- { return convertHumanPoseToPosition(measurements.detectionTime, humanPose); }) |
+ [measurements, this](const armem::human::HumanPose& humanPose) -> DetectedHuman {
+ return convertHumanPoseToDetectedHuman(measurements.detectionTime, humanPose);
+ }) |
ranges::to_vector;
+ // associate the new poses from the measurement with the tracked humans
associateHumans(newPoses);
- // add all not associated humans as new tracked humans
+ // add all not associated poses as new tracked humans
for (const auto& detectedHuman : newPoses)
{
- if (!detectedHuman.associated)
+ if (not detectedHuman.associated)
{
//add new tracked human to list of tracked humans
trackedHumans.push_back(TrackedHuman{
@@ -88,6 +65,81 @@ namespace armarx::navigation::human
}
}
+ /**
+ * @brief HumanTracker::getTrackedHumans Returns all humans that are currently tracked.
+ * @return the tracked humans
+ */
+ std::vector<human::Human>
+ HumanTracker::getTrackedHumans() const
+ {
+ return trackedHumans |
+ ranges::views::transform([](const TrackedHuman& h) -> human::Human
+ { return h.humanFilter.get(); }) |
+ ranges::to_vector;
+ }
+
+ /**
+ * @brief HumanTracker::reset Resets this instance to the same state as if it just would have
+ * been created.
+ */
+ void
+ HumanTracker::reset()
+ {
+ trackedHumans.clear();
+ }
+
+
+ /**
+ * @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)
+ {
+ 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};
+ }
+
+
+ /**
+ * @brief The PosDistance struct contains a distance between an old, tracked human and a new,
+ * detected human aswell as references to them.
+ */
struct PosDistance
{
HumanTracker::TrackedHuman* oldHuman;
@@ -95,6 +147,14 @@ namespace armarx::navigation::human
float distance;
};
+ /**
+ * @brief getSortedDistances Returns all distances sorted by their numeric value between
+ * possible combinations (T, D) where T is an old, tracked human and D is a new, detected human
+ * and T as well as D were not already associated
+ * @param oldHumans the old, tracked humans
+ * @param newHumans the new, detected humans
+ * @return the sorted distances, where the smallest distance is the first entry in the vector
+ */
std::vector<PosDistance>
getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
std::vector<HumanTracker::DetectedHuman>& newHumans)
@@ -113,6 +173,8 @@ namespace armarx::navigation::human
{
continue;
}
+ // calculate distance between every possible combination of tracked and detected
+ // humans where none of them was associated
posDistances.push_back(
{&oldHuman,
&newHuman,
@@ -121,6 +183,7 @@ namespace armarx::navigation::human
}
}
+ // sort the distances ascending by their numeric value
std::sort(posDistances.begin(),
posDistances.end(),
[](const PosDistance& a, const PosDistance& b) -> bool
@@ -129,10 +192,16 @@ namespace armarx::navigation::human
return posDistances;
}
+ /**
+ * @brief HumanTracker::associateHumans Associates those tracked and detected humans that
+ * belong together.
+ * @param detectedHumans The detected humans against which the saved list of tracked humans is
+ * matched.
+ */
void
HumanTracker::associateHumans(std::vector<DetectedHuman>& detectedHumans)
{
- // associate humans by their tracking id
+ // first, associate humans by their tracking id
for (auto& oldHuman : trackedHumans)
{
if (oldHuman.associated || !oldHuman.trackingId)
@@ -145,6 +214,8 @@ namespace armarx::navigation::human
{
continue;
}
+ // check for every possible combination of old and new humans (that were not already
+ // associated and have a tracking id) if their tracking id is the same
if (oldHuman.trackingId.value() == newHuman.trackingId.value())
{
associate(&oldHuman, &newHuman);
@@ -152,8 +223,7 @@ namespace armarx::navigation::human
}
}
-
- // associate leftover humans by their distances
+ // second, associate leftover humans by their distances
const auto sortedDistances = getSortedDistances(trackedHumans, detectedHumans);
for (auto& posDistance : sortedDistances)
@@ -166,10 +236,17 @@ namespace armarx::navigation::human
{
continue;
}
+ // associate the pair with the currently smallest distance between non-associated humans
associate(posDistance.oldHuman, posDistance.newHuman);
}
}
+ /**
+ * @brief HumanTracker::associate Associates the given tracked and detected human. Therefore it
+ * updates all necessary variables of the TrackedHuman
+ * @param trackedHuman the tracked human
+ * @param detectedHuman the detected human
+ */
void
HumanTracker::associate(TrackedHuman* trackedHuman, DetectedHuman* detectedHuman)
{
@@ -183,20 +260,4 @@ namespace armarx::navigation::human
trackedHuman->trackingId = detectedHuman->trackingId;
}
- std::vector<human::Human>
- HumanTracker::getTrackedHumans() const
- {
- return trackedHumans |
- ranges::views::transform([](const TrackedHuman& h) -> human::Human
- { return h.humanFilter.get(); }) |
- ranges::to_vector;
- }
-
-
- void
- HumanTracker::reset()
- {
- trackedHumans.clear();
- }
-
} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/HumanTracker.h b/source/armarx/navigation/human/HumanTracker.h
index 3c4cc28f4b45353fc9d1e4fcbc6616c6e9e4bafc..311817b45b862e38d716441f60181cae25f13700 100644
--- a/source/armarx/navigation/human/HumanTracker.h
+++ b/source/armarx/navigation/human/HumanTracker.h
@@ -39,6 +39,12 @@ namespace armarx::navigation::human
using Vector = Eigen::Matrix<T, 2, 1>;
using SystemModelT = kalman_filter::SystemModelSO2xR2<T>;
+ /**
+ * @brief The HumanTracker class can be used to track and filter multiple humans. It hides
+ * implementation detail on how new detected humans are associated to the old, already tracked
+ * humans. New detected humans can be fed by using the update method. The tracked humans can
+ * be obtained using the getTrackedHumans method.
+ */
class HumanTracker
{
public:
@@ -67,11 +73,14 @@ namespace armarx::navigation::human
struct Parameters
{
- // the duration after which tracked humans will be erased if no new measurement for this human is found
+ // the duration after which tracked humans will be erased if no new measurement for
+ // this human is found
Duration maxTrackingAge = Duration::MilliSeconds(500);
- // the maximum distance in millimeters of two human measurements to be associated with each other
+ // the maximum distance in millimeters of two human measurements where they are still
+ // associated with each other
float maxAssociationDistance = 600;
- // alpha value from interval [0,1] to determine how much the new (and respectively the old) velocity should be weighted
+ // alpha value from interval [0,1] to determine how much the current (and respectively
+ // the old) velocity should be weighted when calculating the new velocity
float velocityAlpha = 0.7;
};
@@ -84,6 +93,9 @@ namespace armarx::navigation::human
private:
void associateHumans(std::vector<DetectedHuman>& detectedHumans);
void associate(TrackedHuman* tracked, DetectedHuman* detected);
+ HumanTracker::DetectedHuman
+ convertHumanPoseToDetectedHuman(const DateTime& time,
+ const armem::human::HumanPose& humanPose);
private:
std::vector<TrackedHuman> trackedHumans;