From 23b0fdd9be34bf4d08a63697f5af62cdc8730561 Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Mon, 7 Nov 2022 16:23:06 +0100
Subject: [PATCH] Implement new cluster center function as ellipsoids are 3d
objects
---
source/armarx/navigation/human/HumanTracker.cpp | 12 ++++++++++++
source/armarx/navigation/human/HumanTracker.h | 7 +++++++
2 files changed, 19 insertions(+)
diff --git a/source/armarx/navigation/human/HumanTracker.cpp b/source/armarx/navigation/human/HumanTracker.cpp
index 4cde5d0d..1c29cf40 100644
--- a/source/armarx/navigation/human/HumanTracker.cpp
+++ b/source/armarx/navigation/human/HumanTracker.cpp
@@ -339,6 +339,18 @@ namespace armarx::navigation::human
return std::max(cluster.ellipsoid.radii.x(), cluster.ellipsoid.radii.y());
}
+ Eigen::Vector2f
+ HumanTracker::getClusterCenter(Cluster cluster)
+ {
+ Eigen::Vector2f center;
+ for (auto& point : cluster.points)
+ {
+ center += point;
+ }
+ center /= cluster.points.size();
+ return center;
+ }
+
void
HumanTracker::prepareTrackedHumans(DateTime time)
{
diff --git a/source/armarx/navigation/human/HumanTracker.h b/source/armarx/navigation/human/HumanTracker.h
index b31ae524..ab360521 100644
--- a/source/armarx/navigation/human/HumanTracker.h
+++ b/source/armarx/navigation/human/HumanTracker.h
@@ -200,6 +200,13 @@ namespace armarx::navigation::human
* @return the size
*/
float getClusterSize(Cluster cluster);
+ /**
+ * @brief HumanTracker::getClusterCenter Returns the center of the given cluster. That is
+ * calculated as the mean of all points.
+ * @param cluster The cluster whose center is calculated
+ * @return the center
+ */
+ Eigen::Vector2f HumanTracker::getClusterCenter(Cluster cluster);
/**
* @brief prepareTrackedHumans Deletes all tracked humans that received no update for too
* long. All remaining tracked humans are prepared for association with the given point in
--
GitLab