Skip to content
Snippets Groups Projects

Draft: Implement human grouping

Open Timo Weberruß requested to merge implement-human-groups into dev
2 files
+ 44
2
Compare changes
  • Side-by-side
  • Inline
Files
2
@@ -2,16 +2,53 @@
namespace armarx::navigation::components::dynamic_scene_provider
{
OrientationDistance::OrientationDistance(double max, double min)
{
this->max = max;
this->min = min;
}
double OrientationDistance::computeDistance(Human &h1, Human &h2)
{
// ranges from 0 to 1
double factor = getOrientationFactor(h1, h2);
OrientationDistance::OrientationDistance(double max, double min)
return min + factor * (max - min);
}
double OrientationDistance::getOrientationFactor(Human &h1, Human &h2)
{
double lineOrientation = getLineOrientation(h2, h1);
double angleH1 = Eigen::Rotation2Dd(h1.pose.linear()).angle();
double angleH2 = Eigen::Rotation2Dd(h2.pose.linear()).angle();
// assuming the angles to be in the interval [0, 2pi]
double deviationFirst = std::abs(normOrientation(angleH1 - lineOrientation));
double deviationSecond = std::abs(normOrientation(angleH2 - (lineOrientation + 1)));
// ranges from 0 (looking directly at each other) to 1 (back to back)
return (deviationFirst + deviationSecond) / 2;
}
double OrientationDistance::computeDistance(Human &h1, Human &h2)
double OrientationDistance::getLineOrientation(Human &h2, Human &h1)
{
double dx = (h1.pose.translation() - h2.pose.translation()).x();
double dy = (h1.pose.translation() - h2.pose.translation()).y();
double lineOrientation = atan2(dy, dx);
return lineOrientation;
}
// scales orientation to (-1, 1]
double OrientationDistance::normOrientation(double orientation)
{
double normedOrientation = orientation / 3.1416;
// brings orientation to [0, 2)
normedOrientation = std::fmod(((std::fmod(normedOrientation, 2)) + 2), 2);
// brings orientation to [-1, 1)
normedOrientation -= (normedOrientation > 1) * 2;
return normedOrientation;
}
}
Loading