From 163e91cf13030b5c83e4680a8f6269a5656a943c Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Mon, 7 Nov 2022 16:44:54 +0100
Subject: [PATCH] Use new implemented features, remove todos
---
.../armarx/navigation/human/HumanTracker.cpp | 21 +++++--------------
1 file changed, 5 insertions(+), 16 deletions(-)
diff --git a/source/armarx/navigation/human/HumanTracker.cpp b/source/armarx/navigation/human/HumanTracker.cpp
index 1c29cf40..506bb660 100644
--- a/source/armarx/navigation/human/HumanTracker.cpp
+++ b/source/armarx/navigation/human/HumanTracker.cpp
@@ -45,8 +45,6 @@ namespace armarx::navigation::human
std::vector<Cluster>
HumanTracker::update(const LaserMeasurement& measurements)
{
- // TODO do we receive cluster with correct threshold / expected size / as accurate as that
- // they distinguish multiple footprints?
prepareTrackedHumans(measurements.detectionTime);
@@ -56,14 +54,10 @@ namespace armarx::navigation::human
std::vector<Cluster> unusedClusters;
for (const auto& cluster : measurements.clusters)
{
-
if (getClusterSize(cluster) <= 2 * parameters.maxFootprintSize)
{
- // TODO: Why is the ellipse translation() here 3D? Can we just use the first 2 entries?
- Eigen::Vector2f clusterCenter = cluster.ellipsoid.pose.translation().segment(0, 2);
-
potentialFootprints.push_back(AdvancedCluster{
- .center = clusterCenter, .cluster = cluster, .associated = false});
+ .center = getClusterCenter(cluster), .cluster = cluster, .associated = false});
}
else
{
@@ -97,15 +91,10 @@ namespace armarx::navigation::human
auto& footprint1 = posDistance.oldHuman->associatedCluster->cluster;
auto& footprint2 = posDistance.newCluster->cluster;
- // TODO: Why is the ellipse translation() here 3D? Can we just use the first 2 entries?
- Eigen::Vector2f centerPos =
- (footprint1.ellipsoid.pose.translation().segment(0, 2) +
- footprint2.ellipsoid.pose.translation().segment(0, 2)) /
- 2;
-
//create pose with orientation from old human
core::Pose2D pose = core::Pose2D::Identity();
- pose.translation() = centerPos;
+ pose.translation() =
+ (getClusterCenter(footprint1) + getClusterCenter(footprint2)) / 2;
pose.linear() = posDistance.oldHuman->humanFilter.get().pose.linear();
@@ -336,14 +325,14 @@ namespace armarx::navigation::human
float
HumanTracker::getClusterSize(Cluster cluster)
{
- return std::max(cluster.ellipsoid.radii.x(), cluster.ellipsoid.radii.y());
+ return 2 * cluster.circle.radius;
}
Eigen::Vector2f
HumanTracker::getClusterCenter(Cluster cluster)
{
Eigen::Vector2f center;
- for (auto& point : cluster.points)
+ for (auto& point : cluster.convexHull)
{
center += point;
}
--
GitLab