diff --git a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
index c53238dbda131cc64b1dd29763a517e88c90c552..d9dce5d071a86b134c879b9ff1d25f99e1a2e96a 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
+++ b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
@@ -35,6 +35,8 @@ armarx_add_component(dynamic_scene_provider
## RobotAPICore
## RobotAPIInterfaces
## RobotAPIComponentPlugins # For ArViz and other plugins.
+ DEPENDENCIES_PRIVATE
+ range-v3::range-v3
# DEPENDENCIES_LEGACY
## Add libraries that do not provide any targets but ${FOO_*} variables.
# FOO
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
index 6bae4e5d8374ea064f687d9b70009d73c016bb4d..279d579d1c0fe590dba13db38544127fa8fd3c05 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -3,97 +3,172 @@
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include <range/v3/range/conversion.hpp>
+#include <range/v3/view/transform.hpp>
+
namespace armarx::navigation::components::dynamic_scene_provider
{
- void
- HumanTracker::update(const Measurements& measurements)
- {
- trackedHumans.clear();
- auto positions = getMeasurementPositions(measurements);
+ HumanTracker::DetectedHuman
+ convertHumanPoseToPosition(const armem::human::HumanPose& humanPose)
+ {
+ const armem::human::Keypoint3DIdMap& keypoints = humanPose.keypoint3dMap;
+ ARMARX_CHECK_NOT_EMPTY(keypoints);
- for (const armem::human::HumanPose& humanPose : measurements.humanPoses)
+ auto centerPos = core::Position::Zero();
+ for (const auto& [_, v] : keypoints)
{
- auto centerPos = positions.find(humanPose);
-
- core::Pose2D pose = core::Pose2D::Identity();
- pose.translation() = Eigen::Vector2f{centerPos.x(), centerPos.y()};
- pose.linear() = Eigen::Rotation2Df(0 /*angle*/).toRotationMatrix();
+ centerPos += v.positionGlobal;
+ }
+ centerPos /= static_cast<float>(keypoints.size());
- const human::Human newHuman = {
- .pose = pose,
- .linearVelocity = Eigen::Vector2f::Zero(), //TODO more sophisticated guess
- .detectionTime = measurements.detectionTime};
+ core::Pose2D pose = core::Pose2D::Identity();
+ pose.translation() = centerPos;
+ pose.linear() = Eigen::Rotation2Df(0 /*angle*/).toRotationMatrix();
- trackedHumans.push_back(newHuman);
- }
+ return pose;
}
- const std::vector<human::Human>&
- HumanTracker::getTrackedHumans() const
+ void
+ HumanTracker::update(const Measurements& measurements)
{
- return trackedHumans;
- }
+ //TODO: proper time to live
+ Duration maxAge = Duration::MilliSeconds(500);
+ for (auto it = trackedHumans.begin(); it != trackedHumans.end();)
+ {
+ auto& human = *it;
+ if ((measurements.detectionTime - human.human.detectionTime) >= maxAge)
+ {
+ it = trackedHumans.erase(it);
+ }
+ else
+ {
+ human.associated = false;
+ human.human.pose = human.human.estimateAt(measurements.detectionTime);
+ it++;
+ }
+ }
- std::map<const armem::human::HumanPose&, armarx::navigation::core::Position>
- HumanTracker::getMeasurementPositions(const Measurements& measurements) const
- {
- std::map<armem::human::HumanPose&, armarx::navigation::core::Position> map;
+ std::vector<DetectedHuman> newPoses = measurements.humanPoses |
+ ranges::views::transform(convertHumanPoseToPosition) |
+ ranges::to_vector;
- for (const armem::human::HumanPose& humanPose : measurements.humanPoses)
- {
+ associateHumans(newPoses);
+ }
- const armem::human::Keypoint3DIdMap& keypoints = humanPose.keypoint3dMap;
- ARMARX_CHECK_NOT_EMPTY(keypoints);
+ struct PosDistance
+ {
+ HumanTracker::TrackedHuman& oldHuman;
+ HumanTracker::DetectedHuman& newHuman;
+ const float distance;
+ };
+
+ std::vector<PosDistance>
+ getSortedDistances(const std::vector<HumanTracker::TrackedHuman>& oldHumans,
+ const std::vector<HumanTracker::DetectedHuman>& newHumans)
+ {
+ std::vector<PosDistance> posDistances;
- auto centerPos = armarx::navigation::core::Position::Zero();
- for (const auto& [_, v] : keypoints)
+ for (const auto& oldHuman : oldHumans)
+ {
+ if (oldHuman.associated)
{
- centerPos += v.positionGlobal;
+ continue;
+ }
+ for (const auto& newHuman : newHumans)
+ {
+ if (newHuman.associated)
+ {
+ continue;
+ }
+ posDistances.emplace_back(
+ oldHuman,
+ newHuman,
+ (newHuman.pose.translation() - oldHuman.human.pose.translation()).norm());
}
- centerPos /= static_cast<float>(keypoints.size());
-
- map.insert(
- std::pair<const armem::human::HumanPose&, armarx::navigation::core::Position>(
- humanPose, centerPos));
}
- }
+ std::sort(posDistances.begin(),
+ posDistances.end(),
+ [](const PosDistance& a, const PosDistance& b) -> bool
+ { return a.distance < b.distance; });
- std::map<armem::human::HumanPose&, human::Human&>
- HumanTracker::getAssociatedHumans(const Measurements& measurements) const
- {
+ return posDistances;
}
- std::vector<human::Human>
- HumanTracker::getMostRecentTrackedHumans() const
+ void
+ HumanTracker::associateHumans(std::vector<DetectedHuman>& detectedHumans)
{
- if (trackedHumans.empty())
+ // associate humans by their tracking id
+ for (auto& oldHuman : trackedHumans)
{
- return {};
+ if (oldHuman.associated || !oldHuman.trackingId)
+ {
+ continue;
+ }
+ for (auto& newHuman : detectedHumans)
+ {
+ if (newHuman.associated)
+ {
+ continue;
+ }
+ if (oldHuman.trackingId.value() == newHuman.trackingId)
+ {
+ associate(oldHuman, newHuman);
+ }
+ }
}
- std::sort(trackedHumans.begin(),
- trackedHumans.end(),
- [](human::Human a, human::Human b) -> bool
- { return a.detectionTime.operator<=(b.detectionTime); });
- DateTime mostRecentTime = trackedHumans.back().detectionTime;
- std::vector<human::Human> mostRecentHumans;
+ // associate leftover humans by their distances
+ const auto sortedDistances = getSortedDistances(trackedHumans, detectedHumans);
- for (human::Human trackedHuman : trackedHumans)
+ for (auto& posDistance : sortedDistances)
{
- if (trackedHuman.detectionTime == mostRecentTime)
+ if (!posDistance.oldHuman.associated || !posDistance.newHuman.associated)
{
- mostRecentHumans.push_back(trackedHuman);
+ continue;
}
+ associate(posDistance.oldHuman, posDistance.newHuman);
}
+ }
- return mostRecentHumans;
+ void
+ HumanTracker::associate(TrackedHuman& trackedHuman, DetectedHuman& detectedHuman)
+ {
+ ARMARX_CHECK(!trackedHuman.associated);
+ ARMARX_CHECK(!detectedHuman.associated);
+
+ trackedHuman.associated = true;
+ detectedHuman.associated = true;
+
+ // TODO alpha parameter
+ float a = 0.7;
+
+ float dt =
+ (detectedHuman.detectionTime - trackedHuman.human.detectionTime).toSecondsDouble();
+ Eigen::Vector2f ds =
+ (detectedHuman.pose.translation() - trackedHuman.human.pose.translation());
+ Eigen::Vector2f linVelocity = ds / dt;
+
+ Eigen::Vector2f velocity = a * linVelocity + (1 - a) * trackedHuman.human.linearVelocity;
+
+ trackedHuman.human = {detectedHuman.pose, velocity, detectedHuman.detectionTime};
+ trackedHuman.trackingId = detectedHuman.trackingId;
}
+ std::vector<human::Human>
+ HumanTracker::getTrackedHumans() const
+ {
+ return trackedHumans |
+ ranges::views::transform([](const TrackedHuman& h) -> human::Human
+ { return h.human; }) |
+ ranges::to_vector;
+ }
+
+
void
HumanTracker::reset()
{
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
index 76c6ce505b0f5ebc7b158745b8c9518aa85bd513..132464db78b372f2ac2f1c02a319aaad1240f5a4 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
@@ -22,21 +22,32 @@ namespace armarx::navigation::components::dynamic_scene_provider
std::vector<armem::human::HumanPose> humanPoses;
};
+ struct DetectedHuman
+ {
+ core::Pose2D pose;
+ std::string trackingId;
+ DateTime detectionTime;
+ bool associated;
+ };
+
+ struct TrackedHuman
+ {
+ human::Human human;
+ std::optional<std::string> trackingId = std::nullopt;
+ bool associated;
+ };
+
void update(const Measurements& measurements);
- const std::vector<human::Human>& getTrackedHumans() const;
+ std::vector<human::Human> getTrackedHumans() const;
void reset();
private:
- std::vector<human::Human> trackedHumans;
-
- std::map<const armem::human::HumanPose&, armarx::navigation::core::Position>
- getMeasurementPositions(const Measurements& measurements) const;
+ void associateHumans(std::vector<DetectedHuman>& detectedHumans);
+ void associate(TrackedHuman& tracked, DetectedHuman& detected);
- std::map<armem::human::HumanPose&, human::Human&>
- getAssociatedHumans(const Measurements& measurements) const;
-
- std::vector<human::Human> getMostRecentTrackedHumans() const;
+ private:
+ std::vector<TrackedHuman> trackedHumans;
};
} // namespace armarx::navigation::components::dynamic_scene_provider