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

Implement OrientationDistance

parent e6c6e2f8
No related branches found
No related tags found
2 merge requests!109Social layers,!55Draft: Implement human grouping
......@@ -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;
}
}
......@@ -10,5 +10,10 @@ namespace armarx::navigation::components::dynamic_scene_provider
public:
OrientationDistance(double max = 1, double min = 0);
virtual double computeDistance(Human &h1, Human &h2);
private:
double getOrientationFactor(Human &h1, Human &h2);
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