Skip to content
Snippets Groups Projects
Commit 7c6a6135 authored by Timo Weberruß's avatar Timo Weberruß
Browse files

Implement ConvexHullGenerator

parent ac6d045e
No related branches found
No related tags found
2 merge requests!109Social layers,!55Draft: Implement human grouping
This commit is part of merge request !55. Comments created here will be created in the context of that merge request.
#include "ConvexHullGenerator.h"
ConvexHullGenerator::ConvexHullGenerator()
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/multi/geometries/register/multi_point.hpp>
using Point = std::pair<double, double>;
BOOST_GEOMETRY_REGISTER_POINT_2D(Point, double, boost::geometry::cs::cartesian, first, second)
namespace armarx::navigation::human
{
ConvexHullGenerator::ConvexHullGenerator()
{
}
shapes::Polygon ConvexHullGenerator::createShape(const HumanGroup &group)
{
using BoostPoly = boost::geometry::model::polygon<Point>;
using GroupPoly = shapes::Polygon;
std::vector<Point> coordinates;
for (Human human : group.humans)
{
coordinates.push_back({human.pose.translation().x(), human.pose.translation().y()});
}
BoostPoly poly, hull;
poly.outer().assign(coordinates.begin(), coordinates.end());
boost::geometry::convex_hull(poly, hull);
std::vector<Eigen::Vector2f> vertices;
for (auto it = boost::begin(boost::geometry::exterior_ring(hull)); it != boost::end(boost::geometry::exterior_ring(hull)); ++it)
{
double x = boost::geometry::get<0>(*it);
double y = boost::geometry::get<1>(*it);
vertices.push_back(Eigen::Vector2f(x, y));
}
return {.vertices = vertices};
}
}
......@@ -21,6 +21,8 @@
#pragma once
#include "GroupShapeGenerator.h"
#include <armarx/navigation/human/types.h>
......@@ -32,9 +34,11 @@ namespace armarx::navigation::human
* @brief A GroupShapeGenerator that defines the group shape of a set of humans as the
* convex hull of their positions in 2D space.
*/
class ConvexHullGenerator
class ConvexHullGenerator : GroupShapeGenerator
{
public:
ConvexHullGenerator();
virtual shapes::Polygon createShape(const HumanGroup &group);
};
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment