From 26182499c65f3dbb4c9ae04115b92159fcab168c Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Mon, 24 Oct 2022 19:15:29 +0200
Subject: [PATCH] Implement getSortedDistances for distance between humans and
clusters
---
.../armarx/navigation/human/HumanTracker.cpp | 48 ++++++++++++++++---
source/armarx/navigation/human/HumanTracker.h | 34 ++++++++++---
2 files changed, 68 insertions(+), 14 deletions(-)
diff --git a/source/armarx/navigation/human/HumanTracker.cpp b/source/armarx/navigation/human/HumanTracker.cpp
index df05c557..0a21dd29 100644
--- a/source/armarx/navigation/human/HumanTracker.cpp
+++ b/source/armarx/navigation/human/HumanTracker.cpp
@@ -127,11 +127,11 @@ namespace armarx::navigation::human
return {pose, humanPose.humanTrackingId, time, false};
}
- std::vector<HumanTracker::PosDistance>
+ std::vector<HumanTracker::PosHumanDistance>
HumanTracker::getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
std::vector<HumanTracker::DetectedHuman>& newHumans)
{
- std::vector<PosDistance> posDistances;
+ std::vector<PosHumanDistance> posDistances;
for (auto& oldHuman : oldHumans)
{
@@ -158,7 +158,42 @@ 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
+ [](const PosHumanDistance& a, const PosHumanDistance& b) -> bool
+ { return a.distance < b.distance; });
+
+ return posDistances;
+ }
+
+ std::vector<HumanTracker::PosLaserDistance>
+ HumanTracker::getSortedDistances(std::vector<TrackedHuman>& oldHumans,
+ std::vector<Cluster>& newClusters)
+ {
+ std::vector<PosLaserDistance> posDistances;
+
+ for (auto& newCluster : newClusters)
+ {
+ //calculate center of cluster
+ Eigen::Vector2f clusterCenter;
+ for (auto& point : newCluster.points)
+ {
+ clusterCenter += point;
+ }
+ clusterCenter /= newCluster.points.size();
+
+ for (auto& oldHuman : oldHumans)
+ {
+ // calculate distance between every possible combination of tracked human and cluster
+ posDistances.push_back(
+ {&oldHuman,
+ &newCluster,
+ (clusterCenter - oldHuman.humanFilter.get().pose.translation()).norm()});
+ }
+ }
+
+ // sort the distances ascending by their numeric value
+ std::sort(posDistances.begin(),
+ posDistances.end(),
+ [](const PosHumanDistance& a, const PosHumanDistance& b) -> bool
{ return a.distance < b.distance; });
return posDistances;
@@ -221,7 +256,7 @@ namespace armarx::navigation::human
}
float
- HumanTracker::getClusterSize(armem::vision::LaserScannerFeature cluster)
+ HumanTracker::getClusterSize(Cluster cluster)
{
float maxSize = 0;
for (auto it1 = cluster.points.begin(); it1 != cluster.points.end();)
@@ -230,9 +265,8 @@ namespace armarx::navigation::human
for (auto it2 = it1 + 1; it2 != cluster.points.end();)
{
auto& point2 = *it2;
- float xdiff = point2.x() - point1.x();
- float ydiff = point2.y() - point1.y();
- maxSize = std::max(maxSize, std::sqrt(xdiff * xdiff + ydiff * ydiff));
+ //TODO does subtraction work for vectors like this?
+ maxSize = std::max(maxSize, (point2 - point1).norm());
}
}
return maxSize;
diff --git a/source/armarx/navigation/human/HumanTracker.h b/source/armarx/navigation/human/HumanTracker.h
index 2d0ee981..8e543425 100644
--- a/source/armarx/navigation/human/HumanTracker.h
+++ b/source/armarx/navigation/human/HumanTracker.h
@@ -39,6 +39,7 @@ namespace armarx::navigation::human
using T = double;
using Vector = Eigen::Matrix<T, 2, 1>;
using SystemModelT = kalman_filter::SystemModelSO2xR2<T>;
+ using Cluster = armem::vision::LaserScannerFeature;
/**
* @brief The HumanTracker class can be used to track and filter multiple humans. It hides
@@ -60,7 +61,7 @@ namespace armarx::navigation::human
struct LaserMeasurement
{
DateTime detectionTime;
- std::vector<armem::vision::LaserScannerFeature> clusters;
+ std::vector<Cluster> clusters;
};
struct DetectedHuman
@@ -78,13 +79,20 @@ namespace armarx::navigation::human
bool associated;
};
- struct PosDistance
+ struct PosHumanDistance
{
HumanTracker::TrackedHuman* oldHuman;
HumanTracker::DetectedHuman* newHuman;
float distance;
};
+ struct PosLaserDistance
+ {
+ HumanTracker::TrackedHuman* oldHuman;
+ Cluster* newCluster;
+ float distance;
+ };
+
struct Parameters
{
// the keypoint which should be used for calculating the rotation of the humans
@@ -95,6 +103,8 @@ namespace armarx::navigation::human
// the maximum distance in millimeters of two human measurements where they are still
// associated with each other
float maxAssociationDistance = 600;
+ // the maximum size (aka length) in millimeters a footprint can have
+ float maxFootprintSize = 350;
// 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;
@@ -118,8 +128,7 @@ namespace armarx::navigation::human
* @param measurements
* @return
*/
- std::vector<armem::vision::LaserScannerFeature>
- update(const LaserMeasurement& measurements);
+ std::vector<Cluster> update(const LaserMeasurement& measurements);
/**
* @brief HumanTracker::getTrackedHumans Returns all humans that are currently tracked.
* @return the tracked humans
@@ -145,14 +154,25 @@ namespace armarx::navigation::human
* @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
+ * 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>
+ std::vector<PosHumanDistance>
getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
std::vector<HumanTracker::DetectedHuman>& newHumans);
+ /**
+ * @brief getSortedDistances Returns a sorted vector of the distances between every possible
+ * combination (T, C) where T is an old, tracked human and C is a new, detected cluster.
+ * The smallest distance will be the first entry in the vector.
+ * @param oldHumans
+ * @param newClusters
+ * @return
+ */
+ std::vector<PosLaserDistance>
+ getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
+ std::vector<Cluster>& newClusters);
/**
* @brief HumanTracker::associateHumans Associates those tracked and detected humans that
* belong together.
@@ -173,7 +193,7 @@ namespace armarx::navigation::human
* @param cluster The cluster whose size is measured
* @return the size
*/
- float getClusterSize(armem::vision::LaserScannerFeature cluster);
+ float getClusterSize(Cluster cluster);
private:
--
GitLab