Draft: Implement human grouping

Closes #34

source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.cpp

@@ -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;
     }
 }