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

Add distance function and start impl (WIP!)

parent a550747b
No related branches found
No related tags found
2 merge requests!109Social layers,!55Draft: Implement human grouping
Showing
with 137 additions and 7 deletions
...@@ -11,11 +11,18 @@ armarx_add_component(dynamic_scene_provider ...@@ -11,11 +11,18 @@ armarx_add_component(dynamic_scene_provider
ArVizDrawer.cpp ArVizDrawer.cpp
HumanTracker.cpp HumanTracker.cpp
HumanGrouper.cpp HumanGrouper.cpp
DistanceFunctionDecorator.cpp
OrientationDecorator.cpp
EuclideanDistance.cpp
HEADERS HEADERS
Component.h Component.h
ArVizDrawer.h ArVizDrawer.h
HumanTracker.h HumanTracker.h
HumanGrouper.h HumanGrouper.h
DistanceFunction.h
DistanceFunctionDecorator.h
OrientationDecorator.h
EuclideanDistance.h
DEPENDENCIES DEPENDENCIES
# ArmarXCore # ArmarXCore
ArmarXCore ArmarXCore
......
#pragma once
template <typename T>
class DistanceFunction
{
public:
DistanceFunction();
virtual double computeDistance(T& t1, T& t2) = 0;
};
#include "DistanceFunctionDecorator.h"
namespace armarx::navigation::components::dynamic_scene_provider
{
template<typename T>
DistanceFunctionDecorator<T>::DistanceFunctionDecorator(DistanceFunction<T> &subject)
{
this->subject = subject;
}
template<typename T>
double DistanceFunctionDecorator<T>::computeDistance(T &t1, T &t2)
{
return subject.computeDistance(t1, t2);
}
}
#include "DistanceFunction.h"
#pragma once
namespace armarx::navigation::components::dynamic_scene_provider
{
template <typename T>
class DistanceFunctionDecorator : DistanceFunction<T>
{
public:
DistanceFunctionDecorator(DistanceFunction<T>& subject);
virtual double computeDistance(T& t1, T& t2);
private:
DistanceFunction<T> subject;
};
}
#include "DistanceFunction.h"
#include "EuclideanDistance.h"
#include <armarx/navigation/human/types.h>
namespace armarx::navigation::components::dynamic_scene_provider
{
using Human = armarx::navigation::human::Human;
double EuclideanDistance::computeDistance(Human &h1, Human &h2)
{
return (h1.pose.translation() - h2.pose.translation()).norm();
}
}
#include "DistanceFunction.h"
#include "armarx/navigation/human/types.h"
#pragma once
namespace armarx::navigation::components::dynamic_scene_provider
{
using Human = armarx::navigation::human::Human;
class EuclideanDistance : DistanceFunction<Human>
{
public:
EuclideanDistance();
double computeDistance(Human &h1, Human &h2);
};
}
...@@ -9,7 +9,7 @@ namespace armarx::navigation::components::dynamic_scene_provider ...@@ -9,7 +9,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
void HumanGrouper::updateHumans(std::vector<human::Human> &newHumans) void HumanGrouper::updateHumans(std::vector<human::Human> &newHumans)
{ {
currentHumans = newHumans;
} }
std::vector<human::HumanGroup> HumanGrouper::getCurrentGroups() std::vector<human::HumanGroup> HumanGrouper::getCurrentGroups()
......
...@@ -27,10 +27,10 @@ ...@@ -27,10 +27,10 @@
namespace armarx::navigation::components::dynamic_scene_provider namespace armarx::navigation::components::dynamic_scene_provider
{ {
/** /**
* @brief Identifies social interaction groups in a collection of detected humans.
*
* Can identify social interaction groups in a collection of humans. The currently detected * Can identify social interaction groups in a collection of humans. The currently detected
* humans can be set by calling updateHumans. * humans can be set by calling updateHumans.
*
* @brief Identifies social interaction groups in a collection of detected humans.
*/ */
class HumanGrouper class HumanGrouper
{ {
...@@ -41,21 +41,29 @@ namespace armarx::navigation::components::dynamic_scene_provider ...@@ -41,21 +41,29 @@ namespace armarx::navigation::components::dynamic_scene_provider
HumanGrouper(); HumanGrouper();
/** /**
* @brief Sets the currently detected humans.
*
* Sets the currently detected humans. Only these humans will be assumed to currently exist * Sets the currently detected humans. Only these humans will be assumed to currently exist
* when getCurrentGroups() is called the next time. * when getCurrentGroups() is called the next time.
*
* @brief Sets the currently detected humans.
* @param newHumans a vector containing the most recently detected humans * @param newHumans a vector containing the most recently detected humans
*/ */
void updateHumans(std::vector<human::Human>& newHumans); void updateHumans(std::vector<human::Human>& newHumans);
/** /**
* @brief Recognizes groups in the current humans.
*
* Identifies and returns social groups in the humans given to this instance by the last * Identifies and returns social groups in the humans given to this instance by the last
* call to updateHumans. * call to updateHumans.
*
* @brief Recognizes groups in the current humans.
* @return a vector containing the recognized human groups * @return a vector containing the recognized human groups
*/ */
std::vector<human::HumanGroup> getCurrentGroups(); std::vector<human::HumanGroup> getCurrentGroups();
struct DistanceFunction
{
};
private:
std::vector<human::Human> currentHumans;
}; };
} // namespace armarx::navigation::components::dynamic_scene_provider } // namespace armarx::navigation::components::dynamic_scene_provider
#include "OrientationDecorator.h"
namespace armarx::navigation::components::dynamic_scene_provider
{
OrientationDecorator::OrientationDecorator(DistanceFunction<Human> &subject, double influenceFactor)
: DistanceFunctionDecorator<Human>(subject)
{
this->influenceFactor = influenceFactor;
}
double OrientationDecorator::computeDistance(Human &t1, Human &t2)
{
double orientationFactor = 1; // TODO: implement
return 0;
}
}
#pragma once
#include "DistanceFunctionDecorator.h"
#include <armarx/navigation/human/types.h>
namespace armarx::navigation::components::dynamic_scene_provider
{
using Human = armarx::navigation::human::Human;
class OrientationDecorator : DistanceFunctionDecorator<Human>
{
public:
OrientationDecorator(DistanceFunction<Human> &subject, double influenceFactor);
virtual double computeDistance(Human &t1, Human &t2);
private:
double influenceFactor;
};
}
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