From e508e128dc61efc775720afd9a24dd4afd97918f Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Mon, 7 Nov 2022 17:35:24 +0100
Subject: [PATCH] Change calculation of cluster center to calculate centroid
---
.../armarx/navigation/human/HumanTracker.cpp | 20 +++++++++++++++----
1 file changed, 16 insertions(+), 4 deletions(-)
diff --git a/source/armarx/navigation/human/HumanTracker.cpp b/source/armarx/navigation/human/HumanTracker.cpp
index 506bb660..1a1fa43f 100644
--- a/source/armarx/navigation/human/HumanTracker.cpp
+++ b/source/armarx/navigation/human/HumanTracker.cpp
@@ -1,5 +1,9 @@
#include "HumanTracker.h"
+#include <boost/geometry.hpp>
+#include <boost/geometry/geometries/point_xy.hpp>
+#include <boost/geometry/geometries/polygon.hpp>
+
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "armarx/navigation/human/types.h"
@@ -331,13 +335,21 @@ namespace armarx::navigation::human
Eigen::Vector2f
HumanTracker::getClusterCenter(Cluster cluster)
{
- Eigen::Vector2f center;
+ typedef boost::geometry::model::d2::point_xy<double> boost_point;
+ typedef boost::geometry::model::polygon<boost_point> boost_polygon;
+
+ std::vector<boost_point> points;
for (auto& point : cluster.convexHull)
{
- center += point;
+ points.push_back(boost_point(point.x(), point.y()));
}
- center /= cluster.points.size();
- return center;
+ boost_polygon poly;
+ boost::geometry::assign_points(poly, points);
+
+ boost_point p;
+ boost::geometry::centroid(poly, p);
+
+ return Eigen::Vector2f{p.x(), p.y()};
}
void
--
GitLab