Add test for combined distance

source/armarx/navigation/human/CombinedDistance.cpp

@@ -15,10 +15,14 @@ namespace armarx::navigation::human
     double
     CombinedDistance::computeDistance(const Human& h1, const Human& h2) const
     {
-        return euclidean.computeDistance(h1, h2)
-               // scales the euclidean distance by a factor in [1, maxOrientationInfluence]
-               * (1 + (maxOrientationInfluence - 1) * orientation.computeDistance(h1, h2))
-               // scales the euclidean distance by a factor in [1, inf) depending on influence
-               * (1 + movementInfluence * movement.computeDistance(h1, h2));
+        double d = euclidean.computeDistance(h1, h2);
+
+        // scales the euclidean distance by a factor in [1, maxOrientationInfluence]
+        d *= (1 + (maxOrientationInfluence - 1) * orientation.computeDistance(h1, h2));
+
+        // scales the euclidean distance by a factor in [1, inf) depending on influence
+        d *= (1 + movementInfluence * movement.computeDistance(h1, h2));
+
+        return d;
     }
 } // namespace armarx::navigation::human

source/armarx/navigation/human/CombinedDistance.h

@@ -33,7 +33,8 @@ namespace armarx::navigation::human
      * @brief The CombinedDistance class combines the standard euclidean distance multiplicatively
      * with an orientation distance in such a way that the distance is always at least the euclidean
      * distance but will be scaled by up to the maximum influence factor if the people face away
-     * from each other.
+     * from each other. The distance will then again be increased by the scaled difference in movement
+     * between the two people. The distance will always be at least the euclidean distance.
      */
     class CombinedDistance : public DistanceFunction
     {

source/armarx/navigation/human/test/human_test.cpp

@@ -40,6 +40,7 @@
 
 #include <armarx/navigation/Test.h>
 #include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/human/CombinedDistance.h>
 #include <armarx/navigation/human/MovementDistance.h>
 #include <armarx/navigation/human/types.h>
 
@@ -47,6 +48,7 @@ using armarx::navigation::core::Pose2D;
 using armarx::navigation::human::EuclideanDistance;
 using armarx::navigation::human::OrientationDistance;
 using armarx::navigation::human::MovementDistance;
+using armarx::navigation::human::CombinedDistance;
 using armarx::navigation::human::Human;
 
 BOOST_AUTO_TEST_CASE(testEuclideanDistance1_standardDistance)
@@ -137,6 +139,28 @@ BOOST_AUTO_TEST_CASE(testMovementDistance1_roughlyMovingTogether)
     BOOST_CHECK_CLOSE(d, 0.5, 0.001);
 }
 
+BOOST_AUTO_TEST_CASE(testCombinedDistance1_threeDistances)
+{
+    const CombinedDistance combined = CombinedDistance(2, 3);
+    Pose2D pose1 = Pose2D::Identity();
+    pose1.translation() = Eigen::Vector2f(0, 0);
+    pose1.linear() = Eigen::Rotation2Df(-M_PI_2).toRotationMatrix();
+    const Human h1 = {.pose = pose1,
+                      .linearVelocity = Eigen::Vector2f(1.0, 1.0),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    Pose2D pose2 = Pose2D::Identity();
+    pose2.translation() = Eigen::Vector2f(2, 0);
+    pose2.linear() = Eigen::Rotation2Df(M_PI_2).toRotationMatrix();
+    const Human h2 = {.pose = pose2,
+                      .linearVelocity = Eigen::Vector2f(1.3, 1.4),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    double d = combined.computeDistance(h1, h2);
+    printf("Distance is %.2f\n", d);
+    BOOST_CHECK_CLOSE(d, 2 * 1.5 * 2.5, 0.001);
+}
+
 //BOOST_AUTO_TEST_CASE(testPathLength)
 //{
 //    armarx::navigation::core::Path path{