From 8dc47d9b811288263ed655b3869f8030dea7fd59 Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Mon, 24 Oct 2022 18:18:44 +0200
Subject: [PATCH] Implement basic version of getClusterSize
---
.../armarx/navigation/human/HumanTracker.cpp | 18 ++++++++++++++++++
source/armarx/navigation/human/HumanTracker.h | 7 +++++++
2 files changed, 25 insertions(+)
diff --git a/source/armarx/navigation/human/HumanTracker.cpp b/source/armarx/navigation/human/HumanTracker.cpp
index c879af3a..df05c557 100644
--- a/source/armarx/navigation/human/HumanTracker.cpp
+++ b/source/armarx/navigation/human/HumanTracker.cpp
@@ -220,4 +220,22 @@ namespace armarx::navigation::human
trackedHuman->trackingId = detectedHuman->trackingId;
}
+ float
+ HumanTracker::getClusterSize(armem::vision::LaserScannerFeature cluster)
+ {
+ float maxSize = 0;
+ for (auto it1 = cluster.points.begin(); it1 != cluster.points.end();)
+ {
+ auto& point1 = *it1;
+ 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));
+ }
+ }
+ return maxSize;
+ }
+
} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/HumanTracker.h b/source/armarx/navigation/human/HumanTracker.h
index d5c92f1c..2d0ee981 100644
--- a/source/armarx/navigation/human/HumanTracker.h
+++ b/source/armarx/navigation/human/HumanTracker.h
@@ -167,6 +167,13 @@ namespace armarx::navigation::human
* @param detectedHuman the detected human
*/
void associate(TrackedHuman* tracked, DetectedHuman* detected);
+ /**
+ * @brief getClusterSize Returns the size of the given cluster. That is for now the maximum
+ * distance between two points.
+ * @param cluster The cluster whose size is measured
+ * @return the size
+ */
+ float getClusterSize(armem::vision::LaserScannerFeature cluster);
private:
--
GitLab