Skip to content
Snippets Groups Projects
Commit b43ce686 authored by Corvin-N's avatar Corvin-N
Browse files

Use new HumanPose definition with TrackingID

parent 87e3ad4e
No related branches found
No related tags found
2 merge requests!38human tracker skeleton,!28Draft: Dev -> Main
...@@ -11,23 +11,29 @@ namespace armarx::navigation::components::dynamic_scene_provider ...@@ -11,23 +11,29 @@ namespace armarx::navigation::components::dynamic_scene_provider
HumanTracker::DetectedHuman HumanTracker::DetectedHuman
convertHumanPoseToPosition(const armem::human::HumanPose& humanPose) convertHumanPoseToPosition(const DateTime& time, const armem::human::HumanPose& humanPose)
{ {
const armem::human::Keypoint3DIdMap& keypoints = humanPose.keypoint3dMap; const std::map<std::string, armem::human::PoseKeypoint>& keypoints = humanPose.keypoints;
ARMARX_CHECK_NOT_EMPTY(keypoints); ARMARX_CHECK_NOT_EMPTY(keypoints);
auto centerPos = core::Position::Zero(); auto centerPos = core::Position::Zero();
int size = 0;
for (const auto& [_, v] : keypoints) for (const auto& [_, v] : keypoints)
{ {
centerPos += v.positionGlobal; if (v.positionGlobal.has_value())
{
centerPos += v.positionGlobal.value().toEigen();
size++;
}
} }
centerPos /= static_cast<float>(keypoints.size()); centerPos /= size;
core::Pose2D pose = core::Pose2D::Identity(); core::Pose2D pose = core::Pose2D::Identity();
pose.translation() = centerPos; pose.translation() = centerPos;
pose.linear() = Eigen::Rotation2Df(0 /*angle*/).toRotationMatrix(); //TODO: angle
pose.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
return pose; return {pose, humanPose.humanTrackingId, time, false};
} }
void void
...@@ -51,9 +57,12 @@ namespace armarx::navigation::components::dynamic_scene_provider ...@@ -51,9 +57,12 @@ namespace armarx::navigation::components::dynamic_scene_provider
} }
} }
std::vector<DetectedHuman> newPoses = measurements.humanPoses | std::vector<DetectedHuman> newPoses =
ranges::views::transform(convertHumanPoseToPosition) | measurements.humanPoses |
ranges::to_vector; ranges::views::transform(
[measurements](const armem::human::HumanPose& humanPose) -> DetectedHuman
{ return convertHumanPoseToPosition(measurements.detectionTime, humanPose); }) |
ranges::to_vector;
associateHumans(newPoses); associateHumans(newPoses);
} }
...@@ -110,11 +119,11 @@ namespace armarx::navigation::components::dynamic_scene_provider ...@@ -110,11 +119,11 @@ namespace armarx::navigation::components::dynamic_scene_provider
} }
for (auto& newHuman : detectedHumans) for (auto& newHuman : detectedHumans)
{ {
if (newHuman.associated) if (newHuman.associated || !newHuman.trackingId)
{ {
continue; continue;
} }
if (oldHuman.trackingId.value() == newHuman.trackingId) if (oldHuman.trackingId.value() == newHuman.trackingId.value())
{ {
associate(oldHuman, newHuman); associate(oldHuman, newHuman);
} }
...@@ -125,9 +134,16 @@ namespace armarx::navigation::components::dynamic_scene_provider ...@@ -125,9 +134,16 @@ namespace armarx::navigation::components::dynamic_scene_provider
// associate leftover humans by their distances // associate leftover humans by their distances
const auto sortedDistances = getSortedDistances(trackedHumans, detectedHumans); const auto sortedDistances = getSortedDistances(trackedHumans, detectedHumans);
//TODO max distance parameter
float maxDistance = 600;
for (auto& posDistance : sortedDistances) for (auto& posDistance : sortedDistances)
{ {
if (!posDistance.oldHuman.associated || !posDistance.newHuman.associated) if (posDistance.distance > maxDistance)
{
break;
}
if (posDistance.oldHuman.associated || posDistance.newHuman.associated)
{ {
continue; continue;
} }
......
...@@ -24,9 +24,9 @@ namespace armarx::navigation::components::dynamic_scene_provider ...@@ -24,9 +24,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
struct DetectedHuman struct DetectedHuman
{ {
core::Pose2D pose; const core::Pose2D pose;
std::string trackingId; const std::optional<std::string> trackingId;
DateTime detectionTime; const DateTime detectionTime;
bool associated; bool associated;
}; };
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment