Skip to content
Snippets Groups Projects

Draft: Implement human grouping

Open Timo Weberruß requested to merge implement-human-groups into dev
2 files
+ 46
2
Compare changes
  • Side-by-side
  • Inline
Files
2
#include "ConvexHullGenerator.h"
#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
{
shapes::Polygon
ConvexHullGenerator::createShape(const HumanGroup& group) const
{
if (group.humans.empty())
{
throw InvalidArgumentException("Human group size has to be at least 1");
}
using BoostPoly = boost::geometry::model::polygon<Point>;
std::vector<Point> coordinates;
for (const Human& human : group.humans)
{
coordinates.emplace_back(human.pose.translation().x(), human.pose.translation().y());
}
BoostPoly poly;
BoostPoly 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)
{
const double x = boost::geometry::get<0>(*it);
const double y = boost::geometry::get<1>(*it);
vertices.emplace_back(x, y);
}
return {.vertices = vertices};
}
} // namespace armarx::navigation::human
Loading