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

Implement grouping algorithm

parent 57987085
No related branches found
No related tags found
2 merge requests!109Social layers,!55Draft: Implement human grouping
......@@ -7,7 +7,7 @@
namespace armarx::navigation::components::dynamic_scene_provider
{
class CombinedDistance : DistanceFunction
class CombinedDistance : public DistanceFunction
{
public:
CombinedDistance(double maxOrientationInfluence);
......
......@@ -5,7 +5,7 @@
namespace armarx::navigation::components::dynamic_scene_provider
{
class EuclideanDistance : DistanceFunction
class EuclideanDistance : public DistanceFunction
{
public:
EuclideanDistance(double min = 0);
......
#include "CombinedDistance.h"
#include "HumanGrouper.h"
namespace armarx::navigation::components::dynamic_scene_provider
{
HumanGrouper::HumanGrouper()
HumanGrouper::HumanGrouper(GroupingSettings settings) : settings(settings)
{
this->distanceFunction = new CombinedDistance(settings.maxOrientationInfluence);
}
HumanGrouper::~HumanGrouper()
{
delete this->distanceFunction;
}
void HumanGrouper::updateHumans(std::vector<human::Human> &newHumans)
......@@ -14,8 +20,40 @@ namespace armarx::navigation::components::dynamic_scene_provider
std::vector<human::HumanGroup> HumanGrouper::getCurrentGroups()
{
DateTime now = DateTime::Now();
}
std::vector<human::HumanGroup> groups = {};
// maps index of human in currentHumans to index of group in groups
std::unordered_map<int, int> humanGroupMap;
for (int parentIdx = 0; parentIdx < currentHumans.size(); parentIdx++)
{
Human parent = currentHumans.at(parentIdx);
// if no group exists yet for this human, create one just for them
if (humanGroupMap.find(parentIdx) == humanGroupMap.end())
{
human::HumanGroup group = {{{}}, {parent}, now};
groups.push_back(group);
humanGroupMap[parentIdx] = groups.size() - 1;
}
for (int childIdx = 0; childIdx < currentHumans.size(); childIdx++)
{
Human child = currentHumans.at(childIdx);
if (this->distanceFunction->computeDistance(parent, child) < settings.groupingThreshold)
{
humanGroupMap[childIdx] = humanGroupMap[parentIdx];
groups.at(humanGroupMap[childIdx]).humans.push_back(currentHumans.at(childIdx));
}
}
}
// TODO: shape generation
return groups;
}
}
......@@ -21,6 +21,7 @@
#pragma once
#include "DistanceFunction.h"
#include "armarx/navigation/core/basic_types.h"
#include "armarx/navigation/human/types.h"
......@@ -34,11 +35,28 @@ namespace armarx::navigation::components::dynamic_scene_provider
*/
class HumanGrouper
{
using Human = armarx::navigation::human::Human;
public:
/**
* @brief The GroupingSettings struct contains settings for group generation.
* This includes the influence of aligned orientation and how aggressively groups
* are generated.
*/
struct GroupingSettings
{
double groupingThreshold;
double maxOrientationInfluence;
};
/**
* @brief Creates a new HumanGrouper
* @param settings different parameters that will affect group generation
*/
HumanGrouper();
HumanGrouper(GroupingSettings settings);
~HumanGrouper();
/**
* @brief Sets the currently detected humans.
......@@ -47,7 +65,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
* when getCurrentGroups() is called the next time.
* @param newHumans a vector containing the most recently detected humans
*/
void updateHumans(std::vector<human::Human>& newHumans);
void updateHumans(std::vector<Human> &newHumans);
/**
* @brief Recognizes groups in the current humans.
......@@ -58,12 +76,11 @@ namespace armarx::navigation::components::dynamic_scene_provider
*/
std::vector<human::HumanGroup> getCurrentGroups();
struct DistanceFunction
{
};
private:
std::vector<human::Human> currentHumans;
void generateShapes(std::vector<human::HumanGroup> &groups);
std::vector<Human> currentHumans;
DistanceFunction *distanceFunction;
GroupingSettings settings;
};
} // namespace armarx::navigation::components::dynamic_scene_provider
}
......@@ -5,13 +5,14 @@
namespace armarx::navigation::components::dynamic_scene_provider
{
class OrientationDistance : DistanceFunction
class OrientationDistance : public DistanceFunction
{
public:
OrientationDistance(double max = 1, double min = 0);
virtual double computeDistance(Human &h1, Human &h2);
private:
double getOrientationFactor(Human &h1, Human &h2);
double getLineOrientation(Human &h2, Human &h1);
double normOrientation(double orientation);
double max;
double min;
......
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